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PREFACE 


Winiatti M. Lear of TRW, under contract to JSC, derived the formulation of 
a Kalman filter and all related transformation matrices and measurement equations 
that are suitable for ground navigation trajectory determination for the space 
shuttle. This work was first presented to JSC in the TRW Technical Report No. 30759 
6002-RU-00 (ref. 3) and is now being converted to this JSC internal note 
by Paul Mitchell of the Mathematical Physics Branch. This material includes 
the following: mathematical equations for processing goundtracking data 

during shuttle ascent and entry, typical error model statistics and other 
constants, and the Fortran listings of a complete bench program that includes 
most of the important features and formulation of the high-speed trajectory 
determination processor adopted by JSC for the real-time space shuttle ascent 
and entry phases. This TRW bench program has been used to perform analysis 
designed to give answers to be used in shuttle planning and design for ascent 
and entry pertaining to ground-based navigation. It can process data from 
three groundtracking stations, two C-band radars, and one S-band radar with 
a total of 10 measurements with common time tags. The measurements are 
C-band 1 range, azimuth and elevation; C-band 2 range, azimuth and elevation; 
and S-band range, Doppler and two angles from either a 30-ft station or an 
85-ft station. 
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GROUND TRACKING DATA PROGRAM DOCUMENT 


SHUTTLE OFT LAUNCH/LANDING 
By William M. Lear 

1.0 INTRODUCTION 

This document gives the equations for processing ground tracking data 
during a Space Shuttle ascent or entry, or any nonfree flight phase of a 
shuttle mission. The resulting computer program will process data from up to 
three stations simultaneously: C-band station number 1, C-band station 
number 2, and an S-band station. The C-band data consists of range, azimuth, 
and elevation angle measurements. The S-band data consists of range, two 
angles, and integrated Doppler data in the form of cycle counts. 

A nineteen element state vector is used in a Kalman filter to process 
the measurements. The first nine elements of the state vector are: 

= position of the shuttle in earth-fixed coordinates, 

_^P = velocity of the shuttle in earth-fixed coordinates, 

= acceleration of the shuttle in earth-fixed coordinates. 

The acceleration components of the shuttle are taken to be independent 

exponentially-correlated random variables. No gravitational acceleration 

is included. Typical statistics for these random variables are a time constant 

2 

of .40 seconds and a standard deviation of 6 m/sec . 

The next nine elements of the state vector are the measurement bias 
errors associated with range and two angles for each tracking station. The 
biases are all modeled as exponentially-correlated random variables with a 
typical time constant of 108 seconds. All time constants are taken to be the 
same for all nine state variables. This simplifies the logic in propagating 
the state error covariance matrix ahead in time. 

The nineteenth element of the state vector is the integration constant 
associated with the integrated Doppler measurements. It is modeled as a 
random walk to account for range rate errors adding to the measurement. 


2.0 STATE VECTOR ELEMENTS 


In more detail, the elements of the state vector are given by 
^EF 

Y^P I ^p, position of the shuttle in earth-fixed coordinates. 
^EF 

^EF 

^EF 1 ^F’ shuttle in earth-fixed coordinates. 

^EF 


tF 


'EF 


■EF 


^F’ acceleration of the shuttle in earth fixed coordinates. 


range bias for first C-band station. 

, azimuth bias for first C-band station, radians. 
Bp.j , elevation bias for first C-band station, radians. 


p2 

'a2 


E2 


, range bias for second C-band station. 

, azimuth bias for second C-band station, radians. 

, elevation bias for second C-band station, radians. 


range bias for S-band station. 

^aX’ ^ bias for S-band station, radians. 

B^y, Y angle bias for S-band station, radians 

Ip, Doppler integration constant for S-band station, cycles. 


\ 

3.0 STATE VECTOR DYNAMICS 


State vector elements 7 through 18 are exponentially correlated 
random variables, assumed constant over the integration step. The dynamical 
equation for an exponentially correlated random variable, e, is 

H = a^i-1 >^1 

a = exp (-aT/t^) 

where 

E[n.j] = 0 

Etn.jnj] = 0 i ?* j 

= 1 i = j 


and where x is the time constant associated with e: t small - a raoidly 

£ E 

changing random variable, x^ large - a slowly changing random variable, is 
the standard deviation of e. Since the best estimate of is zero, the 
filter's best estimate of e is propagated ahead by 

E. = exp(-AT/x^) E._.| 


with the term o vl - a^ n- appearing in the state noise vector, and its 
E 1 

variance appearing in the state noise covariance matrix. 

Thus, in engineering notation, the first nine elements of the filter's 
estimated state vector are integrated ahead with 
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^F,i ^ %F,i-l %,i-l 


%,i ^ %,i-l 


In component form, using state vector notation, the equations for propagating 
the state vector ahead in time are 


^l,i " ^l,i-l ^4,i-l ^ ^7,i-l 


7,i 


^2,i-1 ^5,i-1 ^8,i-l 


X- . = X, . , + X, . , AT + Xq . , ATV2 

3,1 3,1-1 6,1-1 y»i-i 


^4,i " ^4,i-1 ^7,i-1 


^5,i ■ ^5,i-l ■*■ ^8,i-1 


^6,i " ^6,i-l ^ ^9,i-l 


X7.i = "7,1-1 


><8.i = ®"P "8,i-l 
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*9.U ' 

*10, i ” ®*P (-Wtj) 

*ll,i = «*P (-‘T/'b) *11,1-1 

*18,1 ' ®*P '-PT/'b’ *18,1-1 

^19, i " ^19,i-l 

We see that the dynamical equations are linear. That is, we can write 

il ' ^*1-1 

where 0 is the state transition matrix. Let 

= AT P3 = AT^/2 

P.|Q = exp(-ZiT/Tg) PS^^ = exp (-AT/xg) 

Then the state transition matrix can be written as shown below. 
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O'. 



P2 0 0 P3 0 

0 P2 0 n P3 

0 0 P2 0 0 

1 0 0 ?2 0 

0 1 0 0 ?2 

0 0 10 0 
0 0 0 ^lo ^ 

0 0 0 0 P-jp 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 0 0 0 0 

0 C 0 0 0 

0 0 0 0 0 



4.0 THE STATE NOISE COVARIANCE MATRIX 


As mentioned in the previous section, all elements of the state vector 
which are exponentially correlated random variables contribute an element to 
the state noise vector of 


where a = exp (-aT/t^) 

E[n^] = 0 

E[n^. Hj] = 0 i 7^ j 

= 1 i = j 

The Doppler integration constant is modeled differently. Its equation 
is 

'd, 1 ' Vu.i'''' 

where E[n ,-] = 0 

U) , 1 

E[n . n J = 0 i j 

U.l WjJ 

= 1 i = j 


where o n .AT is the state noise term, and is due to intearating the Doppler 

U) _ J rr 

rate bias error, an In order to make the variance of 1^ independent of 
u oj,1 D ' 

AT, we must make a '^-1/ /aT, This is easily seen by writing out 

U) 




CT 

(iJ 




+ 



E[I 





a^ i 
u 


AT' 
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2 

where lAT = T, the total integration time. To make E[I^ .] independent of 

U j I 

AT we must set 
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The state noise covariance matrix is given by S = Let 

P^2 = <^1 n-exp(-2AT/Tg)] 

PS 72 = 1 - exp(-2AT/xg) 

Then the diagonal state noise covariance matrix has the diagonal elements of 


^ 1,1 

^ 2,2 

“ • • • = 

*'^ 6,6 

^7,7 

^S.B 

" ^9,9 

^12 

^ 10,10 

2 

- %1 

PS 72 


^ 11,11 

2 

= "A1 

PS 72 


^ 12,12 

2 

'^El 

PS 72 


^13,13 

2 

'^ P 2 

PS 72 


^14,14 

2 

°A2 

PS72 


^15J5 

2 

0^2 

PS 72 


^16,16 

2 

PS 72 


^17,17 

2 

°aX 

^^72 


^18,18 

2 

= a 

ar 

PS 72 


^19,19 

= KaT 

= P^69 



The value of K for the Space Shuttle is chosen in the following manner. 
From before v/e had 
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= E[i 2_^] . (4 4T) (i4T) 


= E[I 


2 

D,0 


] + KT 


where T is the total time interval. For Apollo the rate bias error was a time- 
wise correlated random variable whose standard deviation was 0.005 cycles/sec. 

We used a time constant of 800 seconds for this random variable, even though 
the actual time constant was less. 

In Apollo the Doppler frequency was multiplied by 1, for the Shuttle the 
Doppler frequency is multiplied by 1000, now giving an apparent rate bias error 
of 5 cycles/sec. Assume now a constant rate bias error acting over 400 seconds, 
then 

K400 = 5^ • 400^ 

K = lOOQO cycles^/sec 

? 2 ’ 

That Ts, this value of K will add a variance of 5“ ■ 400 cycles" over a 
400 second period to the pre-existing variance of the Doppler integration con- 
stant, 
cycles 


For the S-faand frequencies used, the conversion factor from meters to 

2 2 2 2 

is about 15,200 cycles/meter. Thus 5 • 400 = 0.13 meters , not much. 
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5.0 MEASUREMENT EQUATIONS 


5.1 INTRODUCTION 

The equations in this section are taken from reference 1. This reference 
also contains the necessary equations for the C-band stations. 

The subscript EF stands for earth-fixed coordinates whose origin is at 
the center of the earth. The Z^p axis is the earth's axis of rotation, Ypp 
goes through the Greenwich meridian at 0 degrees longitude, and Y^p completes 
the right-handed coordinate system. Position of the Space Shuttle vehicle is 
denoted by 

^ PP = position of vehicle in EF coordinates, the first 3 elements 
of the state vector. 

Position of the tracking antenna is denoted by 

^ PP = position of the antenna in EF coordinates. 

The position of the vehicle with respect to the antenna is denoted by 

%A,EF ^ -^.EF " •^,EF 

The topodetic (subscript TOP) coordinate system has its origin at the 
center of the earth. It is rigidly attached to the earth and is thus rotating 
with the earth. The direction of its axes are referenced to EAST, NORTH, and 
UP at a specific antenna (station) on the surface of the earth. The Zjgp axis 
parallel to the local vertical (normal to the reference ellipsoid) at the antenna. 
XjQp points east and Y-j-gp points north. TOP coordinates are obtained from EF 
coordinates by the constant coordinate transformation matrix, T5. 

-TOP " %F 

The elements of T5 are functions of a particular antenna location. Let p be 
the geodetic latitude of the antenna and x the longitude (+ east). Then the 
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first row of T5 is a unit vector in EF coordinates in the east direction. 


= - sin X 
= cos X 
= 0 

The second row of T5 is a unit vector in EF coordinates in the north direction. 

T 52 ^ = - sin 6 cos a 

1622 “ - sin sin X 

1623 = cos (j) 

The third row of T5 is a unit vector in EF coordinates normal to the surface 
of the reference ellipsoid (the local vertical direction). 


T5 


11 


T5 


12 


T5 


13 


TE^i = cos 1 ^ cos X 


T 522 = cos (j) sin x 


T5 


33 


= s i n $ 


The state vector ^ is in earth-fixed coordinates. Partial derivatives 
of the measurements taken in topodetic coordinates must be converted back to 
EF coordinates. That is, if y* is the measurement, we use 


-V/A JOP °^/A,EF 
S^V.EF ®%A,T0P ^%A,EF ^^.EF 


.3!l 




T5 


where I is the 3 by 3 identity matrix. 
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The measurement refraction correction calculations are shown below. The 
variables appearing in the equations must be generated by the estimated state 
vector. Let 

= n„ - 1 = index of refraction minus 1, the refraction modulus at 

at the antenna site. 

= atmospheric scale height. and are supplied by meteorologists 
at the tracking site. 

R = 6 378 165 meters. Its value is not critical. 

0 

p = geometric (straight line) value of range. Always less than 

the measured value of range, 

E = geometric or true elevation angle. Always less than the measured 

elevation angle, 

Ap = Pj^ - p , refraction correction for range. 

aE = Ef^ - E, refraction correction for elevation angle. 

H = altitude above the tracking site. 

H* = height of a spherical slab atmosphere. 

The refraction corrections are given by 
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Note that for short ranges, of p less than .02 earth radii, the elevation angle 
is restricted to being greater than .6 degree. This is because the equations 
become inaccurate at short ranges and low elevation angles. 


5.2 TWO-WAY RANGE MEASUREMENTS 

Two-way range measurements are made by sending out a "ping" from the radar 
and measuring the round-trip time of the ping from the radar, to the vehicle, 
and back to the radar again. The round-trip time divided by the speed of light 
in a vacuum is the measured range, = p*. If the vehicle has a transponder 
to amplify and return the ping, the transponder may introduce a delay time long 
enough to affect the range measurement. This delay time v^ould look like a 
bias adding to the range measurement. Note that there is no delay time for 
skin tracking. Since skin tracking will be used with the Space Shuttle, we will 
neglect this delay time in our equations. The actual range measurement is given 
by 
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p* - p - pp/c + Ap + B + q 

P 

where 

pp/c = speed of light correction 

“ By/a,EF -^/A EF^^ stands for transpose.) 
c = speed of light in vacuum 
Ap = previously defined refraction correction 

Bp = range bias, an exponentially correlated rangom variable, due to 
instrumentation errors and errors in calculating Ap. 

q = uncorrelated noise 

Typical skin track statistics are 

Tgp = 108 seconds 

For a TPQ-18 or FPQ-6 C-band radar 



12 meters 
6 meters 


For an FPS-16 C-band radar, and for an S-band radar using a transponder on the 
vehicle 


a =18 meters 
Bp 

Oq = 9 meters 
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Neglecting the speed of light and the refraction correction terms, the 
partial derivatives of p* with respect to the elements in the state vector are 


S,EF " Va.EF''IVa.EfI 


3p' 


3Y 




'V/A,EF^ '^/A,EF' 


9p’ 


3Z 


V,EF 


^v/a.ef^I^a.efI 


3p* 

3B 

P 


= 1 


5.3 AZIMUTH AND ELEVATION ANGLE MEASUREMENT 

Azimuth angle, A, and 
elevation angle, E are measured 
with respect to topodetic co- 
ordinates as shown in the 
figure to the right. In this 
figure, A also stands for 
antenna and V means vehicle 
being tracked. The location 
of the vehicle in TOPodetic 
coordinates of EAST, NORTH, UP 
IS denoted by ^V/A,T0P 

Va.top- 



Measured azimuth angle is given 


by 


A* - arctan 


^V/AJOP 

Va,top 


+ B, 


^ ^A 


where 


0 i A* < 360' 


the azimuth angle bias error, an exponentially correlated random 
able, due to hardware errors and unmodeTed refraction effects. 


_ Y 
NORTH 


vari - 
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= the uncorrelated error adding to the measurement. 

The nonzero partial derivatives in topodetic coordinates are given by 


3X 


3A* 

V/A.TOP 


'V/A,T0P 


^V/A,T0P Va.TOP 


8Y 


9A* 

V/A JOP 


'V/A JOP 


2 2 
^V/A JOP ^V/AJOP 


and 

aA* , 

^ ’ 

The topodetic partial derivatives are converted to derivatives with respect 
to ^ as shown in Section 5.1 

Measured elevation angle is given by 

E* = arctan ^ . V/A JOP + + Bp + pp 

y~2 2 ^ ^ 

V ^V/A JOP '^V/AJOP 

where E* can have values of 

0 s E* < 360° 

Note that E* is not negative for angles below the horizon. Also E* will not 
have values greater than 90° except when tracking below the horizon (E = 359.5 
for the radar on a hill.) The value aE is the refraction correction shown in 
Section 5.1. is the bias, an exponentially correlated random variable, due 
to hardware errors and errors in calculating AE, which may be substantial, 
is the uncorrelated error adding to the measurement. 

The partial derivatives of E* in topodetic coordinates (the AE term is 
neglected) are given by iy 



5E* 


SX 


~ ^V/A JOP ^V/A,TOP 


V/AJOP 2 0 70 

P V^W/A TflP ’v 


V/A.TOP ' V/AJOP 


3Y 


3E* 

V/AJOP 


' ^V/AJOP V/AJOP 

2O TO 

P VaJOP V/AJOP 


8E* 


8 Z 


V/AJOP p 


= T_ O ~ 

.2 VN/AJOP + ' V/AJOP 


and 


9E* _ 




= 1 


The topodetic partial derivatives are converted to derivatives with respect 
to ^ as shov/n in Section 5.1. 

For both the azimuth and elevation angle measurements, typical skin 
track statistics are given by 


Tg =103 seconds 
For a TPQ-18 or FPQ-16 C-band radar 

cg = 0.3 miniradians 

c = 0.15 milliradians 

M 

For an FPS-16 C-band radar 
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Og = 0.4 milliradians 
Oq =0.2 milliradians 


5.4 NORTH-SOUTH KEYHOLE ANGLE MEASUREMENT 


The amd oy angle 
measurements are generally 
made with a 9 meter (30 feet) 
S-band antenna. The antenna 
is labeled A and the tracked 
vehicle V. The location of 
the vehicle in TOPodetic co- 
ordinates of EAST, NORTH, UP 
is denoted by 

^V/A,T0P’ "V/A,T0P' 

Measured is given by 



'V/A,T0P 


ay = arctan + Aay + B y + q. 

^ ^V/A,T0P ^ ^ 


where can have values of * 


- 88 ° 5 < 88 ° 


Aa^ is the atmospheric refraction correction. is the bias, an exponentially 
correlated random variable, due to hardware errors and errors in Aa^^. 
the uncorrelated error adding to the measurements 


Let aE be the refraction correction term for E as shown in Section 5.1. Then 
Acy is given by 


^Actually = -10° will read out as 350°, etc. 
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Aa> 


2 

^V/A.TOP 


V^V/A, 


2 2 2 
TOP ^V/A,TOP ^^V/A,TOP ^V/A,TOP^ 


It 

Neglecting Aa^^, the topodetic partial derivatives of ot^ are given by 


3a. 


'V/AJOP 


V/A.TOP 

^V/A.TOP ^V/A,T0P 


3cx 




V/A.TOP 


3av 


3Z 


V/A.TOP 


'V/A.TOP 

ITT 


^V/AJOP ^ ^V/A,T0P 


5B 


= 1 


aX 


The topodetic partial derivatives are converted to derivatives with respect 
to ^ as shov;n in Section 5.1. 


Measured a„ is given by 


oy = arctan 


'V/A.TOP 




2 2 
^V/A,T0P ^-V/A.TOP 


+ AOy + B^y + q 
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1 . 





★ 

where oy is hardware limited to * 

-80° i a* i 80° 

which causes a "keyhole" to exist along the north-south axis. 

Aay is the atmospheric refraction correction. 

B^Y is the bias, an exponentially correlated random variable, due 
to hardware errors and errors in Aoy. 

py is the uncorrelated error adding to the measurement. 

Let aE be the refraction correction term for the elevation angle, E, as shown 
in Section 5.1. Then Aay is given by 




2 

V/A,T0P 


'Va,top ^'/A.TOP 

2 JylZ 2 

^V/A,T0P V^V/A,T0P ^V/A,T0P 


★ 

Neglecting Aay, the topodetic partial derivatives of ay are given by 


^ _ ^V/A,T0P ^V/A,T0P 

^h/AJOP 2 0 70 

"V V/A,TOP ^V/A,TOP 


j /v^ 

2 V^/A.TOP ^V/A,TOP 

P ^ 


3a. 


3Y 


V/A,TOP 


★ 


*Again ay 


= -10° will read out as 350°, etc. 
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^V/AJOP ^V/AJOP 


3a. 


9Z 


V/AJOP 


V^V/A, 


TOP ^V/A,TOP 


and 


3av 


3B 


= 1 


aY 


The topodetic partial derivatives are converted to derivatives with respect 
to Ry as shown in Section 5.1. 

Typical error statistics for both and Oy are given below. For the 
angle bias time constant we have Xg = 108 seconds. 


'^BaX ~ '^BaY " milliradians* 


Z UP 


Oqx = OqY = 0. 4 mi lliradians 


5.5 EAST-WEST KEYHOLE ANGLE MEASUREMENTS 

The and ay angle measurements are 
generally made with an 85 foot S-band 
antenna. The antenna is labeled A and the 
tracked vehicle V. The location of the 
vehicle in TOPodetic coordinates of EAST 
NORTH, UP is denoted by Xy^^^ ^Qp, 

^V/A,T0P ^V/A,T0P* 


*0.4 milliradians for a well calibrated station 



SOUTH 


X EAST 
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Measured is given by 


arctan — + Auy ^ + q 


'V/A,TOP 

•k 

where is hardware limited to a range of 


-88° < ct^ 88° 

Aa^ io the atmospheric refraction correction term 

is the bias, an exponentially correlated random variable, due to 
hardware errors and errors in Lay^. 

is the uncorrelated error adding to the measurement. ^ 

Let AE be the refraction correction term for E as shown in Section 5.1. Then 
Aoy is given by 

= °liWaL,i. 

X r~2 Y 2 2 

V^//A,T0P ^ ^V/A,T0P ^'^V/A,T0P ^ Va,T0P^ 

★ 

Neglecting Aa^, the nonzero topodetic partial derivatives of are 
given by 


*A value of = - 10° would actually read out 350°, etc, 
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3a^ 

3Y 


V/AJOP 


V/A,TOP 
2 2 
Va,TOP ^V/A,TOP 


and 


3Z 


V/A.TOP 


'V/A,TOP 
— 2 2 
^V/A,TOP ^V/A.TOP 


3cxv 


3B 


= 1 


qX 


The topodetic partial derivatives are converted to derivatives with respect to 
cc 3s shown in Section 5.1. 

Measured Cy is given by 


a., = arctan 
r 


^V/A,T0P 




2 2 
’'v/A.TOP ^V/A,T0P 


+ tcy + + c,y 


★ 

ay IS hardware limited to * 

-80° i ay i 80° 

which causes a "keyhole" to exist along the east-west axis. 
*A value of ay = -10° actually reads out 350°, etc. 
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Aoy is the atmospheric refraction correction. 

B^Y is the bias, an exponentially correlated random variable, due to 
hardware errors and errors in Aay. 

Py is the uncorrelated error adding to the measurement. 


Let AE be the refraction correction term for the elevation angle, E, as shown 
in Section 5.1. Then Acty is given by 


Aot. 


W/A,T0P 


^V/A.TOP 




+ Y 


V/A,T0P 'V/A,T0P 




V/A,T0P ^V/A,T0P 


Neglecting Aay, the topodetic partial derivatives of ay are 


8oi. 


'V/A,T0P 


= 1- A2 + 

2 V V/A,T0P nVA.TOP 


9av 


^^V/AJOP A'/A,T0P 


3Y 


V/A,T0P 


\/^V/A,T0P, 


+ Z 


V/A,T0P 


and 


9a^ 

Tl 


^'V/A.TOP Va,T0P 


V/A,T0P 


V V/A,T 


OP ^ ^V/A,T0P 


9a,| 

3B“ 


^ 1 
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The topodetic partial derivatives are converted to derivatives with respect 
to the elements of the state vector, ^ as shown in Section 5.1. 

Typical error statistics for and oiy are given below. For the angle bias 

time constant we have Xg = 108 seconds. 


'^BaX " °BaY 


1 .6 milliradians 


= °qy = 0.4 milliradians 


5.6 INTEGRATED TWO-WAY DOPPLER MEASUREMENTS 

Integrated two-way Doppler measurements at an S-band station are obtained 

Q 

in the following manner. A signal of frequency f^^ % 2*10" cycles/second is 
transmitted from an earth based antenna, A, and received at the vehicle, V, as 
a signal with shifted frequency, fy. This signal is instantaneously retrans- 
mitted with a frequency of kyfy, and received back at the ground station with a 
frequency of f^^^. The frequency of this observed signal is modified by the 
equation 

F = fb " 


in order to increase the precision of the integration procedure. Multiplication 
by the factor k^ increases the precision of the integration procedure, and fj^ 
adds a bias which insures that F never goes negative. 


For the Apollo missions ky = 240/221 , kg =-l , and f^^ = 10 cycles/second. 
The station equipment has been modified for the Space Shuttle missions. For 
The shuttle missions ky = 240/221, kg = 1000, and the biasing frequency is 
240*10^ rycles/second. There will be two transmitted frequencies, f^^, for the 
shuttle. Both frequencies will be about f^^ ^ 2-10^ cycles/second. 
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The basic measurement made at the S-band station is a cycle count, N, 

the integral of the composite frequency, F. It can be shown (ref. 1) that 

the actual cycle count at the observation time T is 

0 

= fb'To - Tqi) - 2 (P + ip - 1 p5) + I„ * 

where 

c = speed of light in a vacuum, 299 792 500 meters/second. 


p, p = range and range rate at T = Tq. 


Ap = range refraction correction 

Tqj = initialization time. Time at which the Doppler integration 
constant is initialized. 

= uncorrelated error adding to the Doppler measurement, ^ 120 
cycles = 8 millimeters. 


Iq = Doppler constant of integration plus the integral of the rate bias 
error, a n . 

u> a. 




0 n dt 

(li 


in “ in •! 1 < 1 

D,i D,1“1 ti.'aj,1-l 

where n has a unit variance and o is the variance of the rate bias error 

U (l) ^ 

(see Section 4). ip is initialized with values of p and N existing at T = T^j, 
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Ip ' (Toi) + 2 -^-7-^P (T„,) 


'or 


Note that the small terms Ap and pp/c are ignored in the initialization and 
Iq is given a large initial variance. 


’ID 


, 4''V*'trf , 2 2 2, 

2 5 } Wx + “y * "z’ 


★ 

The nonzero partial derivatives of N 
state vector are {ap and pp/c are ignored). 


vn'th respect to elements of the 


3N 

^^V,EF 


* 



SN 

°^V,EF 


* 


9M 



= 1 


2 ^^s'^V^tr ^V/A,EF 


c 

V/ n , LI 

p 

^S*'V^tr 

^V/A,EF 

c 

0 

^'s'^V^tr 

^'/A,EF 


c p 
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6.0 THE CONVERSION FROM GEODETIC 


COORDINATES TO EARTH-FIXED COORDINATES 


The station locations are input to the TRW Bench Program in terms of the 
geodetic coordinates: 


= geodetic latitude 
X = east longitude 

h = altitude above the reference ellipsoid 
Let 

= equatorial radius of the reference ellipsoid. 

Rp = polar radius of the reference ellipsoid. 

f = (R^ - Rp)/R^ = flattening or ellipticity of the reference 

ellipsoid. 


Typical values are R^ = 6 378 166 meters and f = 1/298.3. 


The cartesian, earth-fixed coordinates are X^p, Y^p, 


Zpp where X^p and 


Ypp are in the earth's equatorial plane (normal to the mean axis of rotation) 
and Zpp lies along the earth's mean axis of rotation. X^p lies in the plane 
formed by the Greenwich meridian and passes through 0° latitude, 0° longitude. 
Ypp passes through 0° latitude and 90° east longitude. 


The earth-fixed, EF, coordinates are given by 


'EF 


^Jcos^ i + (1 - f sin 


+ h 


cos cos X 
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Though not used by the program, the inverse problem is of interest. To 
find i{), X, h given X^p, Ypp, Zpp is a more difficult problem, x is easily 
obtained from 




using a starting value for B of .0067. Then the geodetic latitude, f, is 
given by 





Altitude, h, is given by 







Note that for h = 0, B ^ 1/(1 - f)’ -l. For h = », B = 0. 



7.0 CONVERSION FROM ECI COORDINATES 


TO EARTH-FIXED COORDINATES 


The onboard telemetry vector comes into the TRW Bench Program in earth- 
centered-inertial (ECI) coordinates. It is necessary to convert this vector 
to earth-fixed (EF) coordinates. 

An ECI coordinate system has its origin at the center of the earth and 
is nonrotating with respect to the fixed stars. In a true inertial coordinate 
system there must be no rotation and a zero acceleration of the origin. Our 
ECI coordinate system will not be a true inertial coordinate system since its 
origin will experience a small acceleration as the earth moves around the sun 
in the solar system. However, since all bodies in the vicinity of the earth 
experience this same gravitational acceleration of the sun (and moon) the effects 
of the acceleration tend to cancel when the motion of the body is expressed 
in ECI coordinates. 


The tranformati on matrix (RNP) relates the Earth-fixed coordinated to the 
ECI coordinates by the following equation at time: T = Tq 


^CI 


(P.HP)'^ R^p 


Since the EF coordinate system rotates around its Z axis, the Z-rotation matrix 
is introduced to give at time T by 

0 - (L'g (T “ Tg) 


^CI 


(RNP)"^ 


cos e -sin 0 0 

sin 0 cos e 0 



Bef 


V 


Where is the earth's angular velocity in inertial space, = .729211514646 
•lO”^ radians/second. Due to precesion and nutation of the earth's axis of 
rotation, RNP was changed slightly every six hours during the Apollo missions. 

The above coordinate transformation matrices are orthogonal, the inverse 
equals the transpose, so 


cos 0 sin e 0 

R^P = - sin 0 cos e 0 (RNP) 


And, taking the derivative with respect to time yields 


cos 0 sine 0 


- sin 0 cos 0 0 (RNP) R 


■sin 0 cos e 0 


oip -cos 0 -sin 0 0 (RNP) 


cos 0 -sin 0 0 


(RNP) “ sin 0 cos 0 
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8.0 THE KALMAN FILTER EQUATIONS 


The Kalman filter equations used by the program are shown below in Table 1. 


1 ) 

0 = 8 f / dX 

form transition matrix 

2 ) 

X = f(>^, t^_^, aT) 

integrate state vector 

3) 

C = 0C 0 '’’ + S 

propagate error covariance matrix’*’ 


DO 14 I = 1. M 

M = number of meas. at time t 

n 

4) 

' 81 <4- 

estimate Ith measurement 

5) 

P = 

form measurement partials 

6 ) 

A = CP''' 

temporary storage vector 

7) 

CALCULATE NONLINEAR CORRECTION TERM aQ. 

8 ) 

R = PA + Qj + aQ 

estimate residual variance 

9) 

LOAD SCALAR MEASUREMENT, y*, 

FOR TIME t . 

n 

10) 

IF (yj - yj)^> 36R GO TO 14 

6a residual edit 

11 ) 

W = A/R 

form residual weighting vector 

12) 

X = A + W(y* - yj) 

correct state vector 

13) 

c = c - wa''' 

correct error covariance matrix'*’ 

14) 

CONTINUE 



G(j) T(ji 1 


'Hake use of symmetry in steps 3 

and 13. 


Table 1. The Kalman Filter Equations 
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Making use of symmetry in steps 3 and 13 is essential for trouble free 
performance of the Kalman filter. Calculate only the lov;er triangular part 
of C, and set the upper triangular part equal to the lower triangular part. 
This is, of course, faster but most importantly it forces C to be a symmetric 
matrix. Theoretically the equations in Table 1 give a symmetric C matrix. 
However, due to computer programming, the sequence of operations for the 
upper and lower parts C are different, and roundoff error can cause C to be- 
come increasingly nonsymmetric. When this happens, the filter may become un- 
stable and blow up. Generally v/hen this occurs, the investigator will find 
negative variances along the diagonal of C. He then may mistakenly assume 
that, since the error covariance is not positive semidefinite, this is the 
reason that the filter blew up. 


When the estimated measurement y - gU, t^) is a highly nonlinear function 
of x_, and when the error in the estimate of x is large, then there may be 
severe difficulty in getting the Kalman filter to converge. What is apparent 
to the investigator is that the error covariance matrix will decrease to 
values that are much smaller than the actual errors in the state vector. The 
filter will tend to ignore the current measurements even though the residual 
maybe large. In these cases a nonlinear correction term, aQ, should be 
calculated. It can be shown that aQ is given by the following equations (ref. 2) 


Let 


Then 


9-i-i = (measurement parti als) 

Ij oX^- OAj 

= The ij element of C 


..n - i ^ rkm ptn ■ , /I „ pKt,2 

AQ - 2 C C g^^^ + (2 9k£ C ) 


where the repeated subscripts and superscripts mean a summation from 1 to N, 
the number of elements in the state vector. 
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The above value of aQ has been found to work very well in the Space 
Shuttle onboard navigation equations. The use of AQ in effect increases the 
value of the measurement noise variance, Q. This causes the error covariance 
matrix, C, to decrease more slowly than normal, and the corrections to the 
state vector are not as large as with no AQ, Since the measurement residuals 
are v/eighted less using AQ, it is referred to as measurement underweighting., 

The value of aQ generally becomes small after the first few measurements are 
processed and can generally (but not always) be ignored thereafter. 

We note that the above equation for aQ is quite complicated. In reference 2 
an alternate, simpler method for calculating AQ is given which we will use in 
the High Speed Tracking Data Processor. For all the measurements except the 
Doppler measurement, when the position error is large aQ is calculated by 


AQ = 0.2 PCP''’ = 0.2 PA 

The residual variance is then given by 


R = 1 . 2 PA + Q 


v.'here 1.2 is the measurement underweighting factor. This factor is set to 1 

when the position error variance, C(l,l) + C(2,2) + C(3,3), is less than 
2 

(1000 meters) . For the Doppler measurement, an underweighting factor of 3 all 
the time was found to be necessary, AQ = 2 PCP^ = 2 PA in this case. 
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9.0 TRW BENCH PROGRAM FLOW DIAGRAM 


^1 




The overall flow diagram for the TRW Bench Program High-Speed-Tracking-Data 
Processor is shown in Figure 1. Each block in the figure (there are seven 
blocks) is a subroutine called by the main program. The PS(I) and P(I) arrays 
mentioned in the figure are input and calculated constants. The NF(I) array 
consists of logic flags. The equation 


^ ' ^^REAL ■ "''lAG^'^^''^ 

means that I is set to the next lowest integer, just as would be done when 
using FORTRAN. In block 6, PBI stands for push button indicator. 
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1 


ZERO PS(I), P(l). AND NF(I) ARRAYS. 

READ IN INPUT CONSTANTS INTO PS(I)’AND P(I). 

SET ASCENT-ENTRY PLAO. NF(20). 

CALCULATE FIXED CONSTANTS IN PS(1) AND P{I) ARRAYS. 
(THIS BLOCK CAN BE EXECUTED AT ANY TIME.) 


-PROGRAK exit/return POINT. ENTER EVERY iT • P(Z) HOURS, 



Vate ■ Vate 


PICK UP STATION ID NUMBERS AND LOAD INTO THE NF{I) ARRAY. 

PICK UP ALL tracking DATA WITH TIME TAG AND STORE IN P(I) ARRAY, 

SET DATA GOOD-BAD (T.O) FLAGS IN NF(I) ARRAY. IF NO DATA Ay TIME 
SET DATA FLAG BAD. IF MANUAL EDIT IS ON SET DATA GOOD-BAD FLAGS « -2. 


IF STATION ID HAS CHANGED, REINITIALIZE THE PS(I) ARRAY, THE STATE VECTOR 
BIASES, AND THE STATE ERROR COVARIANCE MATRIX. IF S-BAND ID HAS CHANGED, 

SET NF',16) • 0 TO INITIALIZE DOPPLER. SET KEYHOLE FLAG NF(I), AND READ IN 
S-BAND FREQUENCY INTO PS(I) ARRAY. SET OLD ID NUMBERS • CURRENT ID NUMBERS. 


INTEGRATE STATE' VECTOR. X, AND THE STATE ERROR 
COVARIANCE MATRIX AHEAD TO TIME T„,,,- 


INITIALIZE OFF ANY GOOD SET OF RANGE AND 2 ANGLES 
DATA. IF INITIALIZED SH NF(19) • 2 AND NF(I8) • 0 
(FOR DOPPLER INITIALIZATION), AND TURN OFF FBI 
INITIALIZATION LIGHTS. 


INITIALIZE OFF ANY CURRENT TELEMETRY VECTOR, SH NF(I9) • 2 AND NF(I8) • 0 
(FOR DOPPLER INITIALIZATION LATER), AND TURN OFF PBI INITIALIZATION LIGHTS, 


FIGURE 1: FLOW DIAGRAM FOR THE HIGH-SPEED-TRACKING-DATA PROCESSOR 
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10.0 TRW BENCH PROGRAM INPUTS AND OTHER CONSTANTS 


The PS(I) and P(I) arrays are used for nondestroyable storage in the 
program. There are 72 elements in each array. The PS (I) array is used for 
tracking station related parameters. The definitions of each element in the 
arrays are shown in Sections 10.1 and 10.2. Where suitable, typical values 
of the elements of the array will be shown. The units used will be earth 
radii (6 378 165 meters), hours, radians, and cycles. (Cycles are used for 
the Doppler measurement.) These are the units used with the actual real-time 
program. However, the program shown in this report may use any system of 
units for length and time, so long as one is consistent throughout the arrays. 
For example, all preliminary design work was done using meters, seconds, 
radians, and cycles. Note that all values used in the arrays are zeroed at 
the beginning of block 1. 

Asterisks will denote the type of constant: 

* premission program input, 

** real time program input, 

no asterisk, parameter determined by program. 

10.1 THE PS (I) ARRAY 

The station characteristics constants for the number 1 C-band station 
are stored in PS(I) 1=1, 20. The station characteristics constants for the 
number 2 C-band station are stored in PSfl) I = 21, 40. The station character- 
istics constants for the S-band station are stored in PS(I) I = 41 , 67. 

**PS(1) = h altitude of the first C-band station above the reference ellipsoid. 
Typical value is 1.2480-10"^ er (796.0 meters). 


tThe reference ellipsoid is specified by the constants P(17) and P(18). 
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**PS(2) = <p, geodetic latitude of the first C-band station with respect to the 
the reference ellipsoid. Typical value would be .61018385 radians. 

**PS(3) = X, east longitude of first C-band station, 4.22524581 radians. 

**PS(4} = refraction modulus for the first C-band station. The in- 

dex of refraction minus 1. This number changes from month to month. A tyoical 

■ M ^»p I— . .1.^ II ^ i.i I m ■■■ ww i — . i» — * 

value would be .0003307, no units. 

**PS(5) = Hg, the atmospheric scale height used in the refraction corrections 
at the first C-band station. Literally the height of a spherical slab 
atmosphere whose refraction modulus is everywhere N^. changes from month 
to month . Typical value = .00104 er (6633 meters). 

**PS(6) = 0 for the range bias at the first C-band station, 2.8-l0~^ er (18 
meters 60 feet) . 

**PS(7) = a for azimuth bias at first C-band station, .0004 radians (0.4mi11i- 
radians). 

**PS(8) = a for elevation angle bias at first C-band station, .0004 radians 
(0.4 mi Hi radians). 

**PS(9) = a for uncorrelated error adding to the range measurement at the 
number 1 C-band station, 1.4-10'^ er (9 meters). 

**PS(10) = a for the uncorrelated error adding to the azimuth measurement at 
the number 1 C-band station, .0002 radians (0,2 milliradians). 

**PS(11) = 0 for the uncorrelated error adding to the elevation angle measure- 
ment at the number 1 C-band station, .0002 radians (0.2 milliradians). 

PS(12) =Qp [1 - exp (-2AT/Tg)], number 1 C-band state noise variance for the 
range bias. 

2 

PS(13) = 0 ^ [1 - exp (-2AT/Tg)], number 1 C-band state noise variance for the 
azimuth bias. 
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2 

PS{14) = 0 ^ [1 - exp(-2AT/xg)] , number 1 C-band state noise variance for the 
elevation angle bias. 

PS(15) = PS(9)**2, variance of the uncorrelated error adding to the range 
measurement for the number 1 C-band station. 

PS(16) = PS(10)**2, variance of the uncorrelated error adding to the azimuth 
measurement for the number 1 C-band station. 

PS(17) = PS(11)**2, variance of the uncorrelated error adding to the elevation 
angle measurement for the number 1 C-band station. 

PS(18) = Xgp 1 

PS(19) = Ypp > Earth-fixed coordinates of the first C-band station. 

PS(20) = Zpp j 

The station characteristics constants for the second C-band station are 
shown below. 

**PS(21) = h, altitude of second C-band station above the reference ellipsoid. 

-5 

Typical value is 1.568*10 er (100 meters). 

**PS(22) = 5 , geodetic latitude of second C-band station with respect to the 
reference ellipsoid. Typical value is .60503594 radians. 

**PS(23) = X, east longitude of second C-band station, 4.1786^945 radians 

**PS(24) = Np = n^-1, modulus of refraction for the second C-band station. 

The index of refraction minus 1. This number changes from month to month . A 
typical value would be .0003307, no units. 

**PS(25) = Hj, the atmospheric scale height used in the refraction corrections 
at the second C-band station. Literally the height of a spherical slab atmos- 
phere whose refraction modulus is everywhere N^. H e changes from month to month . 
Typical value = .00104 er (6633 meters). 
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**PS(26) = a for the range bias at the second C-band station, 1.9*10”^ er 
(12 meters = 40 feet). 

**PS(27) = c for azimiith bias at the second C-band station, .0003 radians (0.3 
mi lliradians). 

**PS(28) = a for elevation angle bias at the second C-band station, .0003 
radians (0.3 milliradians). 

**PS(29) = 0 for uncorrelated error adding to the range measurement at the 
second C-band station, .95*10~^ er (6 meters = 20 feet). 

**PS(30) = cr for uncorrelated error adding to the azimuth measurement at the 
second C-band station, .00015 radians ,(0.15 milliradians). 

**PS(31) = a for uncorrelated error adding to the elevation angle measurements 
at the second C-band station, .00015 radians (0.15 milliradians). 

2 

PS(32) = Op [1 - exp(-2AT/Tg)], second C-band state noise variance for the 
range bias. 

PS(33) = [1 - exp(-2AT/xg)], second C-band state noise variance for the 

azimuth bias. 

PS(34) = [1 - exp(-2AT/xg)], second C-band state noise variance for the 

elevation angle bias. 

PS(35) = PS(29)**2, variance of the uncorrelated error adding to the range 
measurement at the second C-band station. 

PS(36) = PS(30)**2, variance of the uncorrelated error adding to the azimuth 
measurement at the second C-band station. 

PS(37) = PS(31)**2, variance of the uncorrelated error adding to the elevation 
angle measurement at the second C-band station. 


43 


PS(38) = Xgp ] 
PS{39) = Ypp 
PS(40) = Z 


Earth-fixed coordinates of second C-band station. 


-EF ^ 


The station characteristics constants for the S-band station are shown 
below. 


**PS(41) = h, altitude of the S-band station above the reference ellipsoid. 
Typical value is 1.872-10"^ er (1194 meters). 

**PS(42) = ij), geodetic latitude of the S-band station with respect to the refer- 
ence ellipsoid. Typical value is .61202125 radians. 

**PS(43) = X, east longitude of the S-band station, -2.03838859 radians. 

**PS(44) = N = n -1 , modulus of refraction for the S-band station. The index 
0 0 

of refraction minus 1. This number changes from month to month . A typical 
value would be .0003307, no units. 

**PS(45) = Hj, the atmospheric scale height used in the refraction correction 
equations for the S-band site. Literally the height of a spherical slab 
atmosphere whose refraction modulus is everywhere N^. H e changes from month 
to month . Typical value = .00104 er (6633 meters). 

**PS(46) = a for the range bias at the S-band station, 2.8*10'^ er (18 meters). 

**PS(47) = a for the bias at the S-band station, .0016 radians (1.6 milli- 
radians). For a well calibrated station a = ,0004 radians. 

**PS(48) = a for the ay bias at the S-band station^ .,0016 radians (1.6 milli- 
radians). For a well calibrated station a = .0004 radians. 

**PS(49) = 0 for the uncorrelated error adding to the range measurement at 
the S-band station, 2.3-10"^ er (14.5 meters). 
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**PS(50) = o for the uncorrelated error adding to the measurement at the 
S-band station, .0004 (0.4 mil 1i radians). 

**PS(51) = a for the uncorrelated error adding to the cxy measurement at the 
S-band station, .0004 radians (0.4 milliradians). 

**PS(52) = a for the uncorrelated error adding to the Doppler cycle count 
measurement at the S-band station, 120 cycles. 

2 

PS(53) = Op [1 - exp(-2AT/Tg)] , S-band state noise variance for the range 
bias. 

PS (54) 

bias. 

p 

PS(55) = o^Y [1 - exp(-2AT/xg)] . S-band state noise variance for the ay bias. 

PS(56) = PS(4D)**2, variance of the uncorrelated error adding to the range 
measurement at the S-band station. 


= a^y [1 - exp (-2AT/tg)], S-band state noise variance for the cxy^ 


PS(57) = PS(50)**2, variance of the uncorrelated error adding to the ay measure- 
ment at the S-band station. 


PS(53) = PS(5l)**2, variance of the uncorrelated error adding to the oy measure- 
ment at the S-band station. 


PS(59) = PS(52)**2, variance of the uncorrelated error adding to the Dopoler 
cycle count measurements at the S-band station. 


PS(60) = Xgp ■ 
PS(61) = Ypp ■ 
PS(62) = Zpp , 


Earth-fixed coordinates of the S-band station. 


PS(63) = f. , the biasing frequency for the Doppler measurement at the S-band 
^11 * 
station, 3.64-10 cycles/hour (240-10 cycles/second). 
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*PS(64) = ky, s-band frequency multiplication factor at the beacon on the 
vehicle, 240/221 = 1.0S5972851 no units. 

*PS(65) = kj, Doppler frequency multiplication factor at the S-band station, 
1000 no units. 

**PS(66) = f^^,the S-band transmitter frequency appearing in the incoming data 
stream , not the station characteristics table, in units of cycles /second, f^^. 
will be converted to units of cycles/hour external to this program. A typical 
value for the Apollo mission was 7. 566487603-1 o'* ^ cycles/hour (2.101802112-10^ 
cycles/second). 

PS(67) = 2 where c is the speed of light in a vacuum. Typical value 

for P(67) = 9.71258784-10''° cycles/er = 15227.19902 cycles/meter. 

*PS(68) = K in KAT, the state noise variance for the Doppler integration 

7 

constant, U, the 19th element of the state vector. Typical value = 3.6-10 
2 u 2 

cycles /hour = 10000 cycles /second. 

PS (69) = KAT, see above. 

*PS(70) = Tg, time constant for all measurement biases, typical value is .03 
hours = 108 seconds. 

PS(71) = exp(-AT/Tg). 

PS(72) = 1 - PS(71)**2. 

10.2 THE P(I) ARRAY 

The following constants and variables are not related to station para- 
meters. 

P(l) = = Tg, time tag of state vector in hours. 
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*P(2) = AT, nominal time interval between measurements. State vector is 
provided every aT hours whether or not there are currently good measurements 
available. Typical value = 5.5555 55555 55556*10 hours (0.2 seconds). 

P(3) = AT^/2 

*P(4) = Tj^^g = T|^, filter lag time behind real time. Due to transport delay 
in getting data from the tracking site to the computer. Typical value = .0005 
hours (1.8 seconds). 


**P(5) = T 


REAL 


= Tpji^, real current mission time in hours, measured from the 


beginning of the year. 


Nominal measure- 

L T _ 


ment times 


LAG 



\, 

— ^ 

1 

1 

1 


1 • ^ ^ 

I ■^REAL 


"^STATE 


^ "^STATE “ I * AT 


Figure 2: Determination of the Initial Filter State 

Time (Filter called every aT hours there- 
after.) 


*P(6) = ECRV T- for acceleration during ascent.' Typical value = .01111 illll 

a * 

hours (40 seconds). 


t 


ECRV stands for "exponentially correlated random variable". 
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2 

*P(7) = ECRV a for acceleration during ascent. Typical value is 12.2 er/hr 

2 ° 

(6 m/sec ). 

*P(8) = ECRV T. for acceleration during entry. Typical value = .01111 11111 

G 

hours (40 seconds). 

2 

*P(9) = ECRV c- for acceleration during entry. Typical value is 12.2 er/hr 

2 “ 

(6 m/sec ). 

P(10) = exp(“AT/-^) for either ascent or entry phase. 

a 

2 

P(ll) = 0 - for either ascent or entry phase, 

G 

2 

P(12) = c, [1 - exp(-2AT/T,)] for either ascent or entry phase. 

O 0 

*P(13) = position variance for initial, diagonal, state error covariance 
matrix for a restart from the raw tracking data. Typical value = 2.458*10"^ 
er^ (10000 meters)^. 

*P(14) = position variance for initial, diagonal, state error covariance matrix 

2 2 

for a restart from a telemetry vector. Typical value = 1 er (6 378 165 meters) . 

*P(15) = velocity variance for initial, diagonal, state error covariance matrix, 
20.4 er^/hr^ (8000 m/sec)^. 

*P(16) = residual edit parameter. P(16) = 6 edits 6o residuals. 

*P(17) = R^, equatorial radius of the ellipsoidal figure of the earth. For 
the 1960 Fischer ellipsoid = 1.000000157 er (6 378 166 meters). 

*P(18) = f, ell ipti city or flattening of the ellipsoidal figure of the earth. 

For the 1960 Fischer ellipsoid f = 1/298.3 = 3.352329869-10"^ no units. 

*P(19) = c, speed of light in a vacuum, 169210.5802 er/hr (299 792 500. m/sec). 
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, : 


*P(20) = 0 ^, inertial angular velocity of the earth, .262516 145273 rad/hour 
(.729211 514646.10'^ rad/sec). 

**P(21) = time tag of RNP matrix in hours. 

**P(22) " 

**P(23) 

**P(24) 

The RNP matrix, column stored. RNP is used in the conversion 
of the ECI telemetry vector to earth-fixed (EF) coordinates. 

The time tag of this matrix must be for the time = P(21), 


*P(31) = RSS Dosition error variance below which the measurement underweiqht- 

-3 

ing, nonlinear measurement correction, is turned off. Typical value = 2. 458-10 
er^ (1000 meters)". 

P(32) = counter for Doppler bias frequency calculation. 

P(33) = scaled residual^ for range from the first C-band station. 

P(24) = scaled residual for azimuth from the first C-band station, 

P(35) = scaled residual for elevation from the first C-band station. 

P(36) = scaled residual for range from the second C-band station. 

P(37) - scaled residual for azimuth from the second C-band station. 

P(38) = scaled residual for elevation from the second C-band station. 

P(39) = scaled residual for range from the S-band station. 

P(40) = scaled residual for ay from the S-band station. 

P(41) = scaled residual for oty from the S-band station. 

P(42) = scaled residual for cycle count from the S-band station. 


^Residual divided by the filter's predicted standard deviation for the residual. 


**PC25) 

**P(26) 

**P(27) 

**P(28) 

**P(29) 

**P(30) 
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**P(43), **P(44), **P(45) = p. A, E data from the first C-band station in 

er, and radians. DOD EFG data is converted externally to p. A, E data. 

**P(46)ft **P(47), **P(4S) = p, A, E data from the second C-band station in 

er, rad’IiMVwte and radians. DOD EFG data is converted externally to p. A, E 

data. 

**P(49), **P(50), **P(51), **P(52) = P, a^, Oy, and Doppler cycle count from 
the 5-band station. Units are er, radians, radians, and cycle counts. 

**P(I) I = 53, 58 is telemetry vector of position and velocity in M50 ECI 
coordinates. Time tag is not important. 

P(59) * 1. or 1.2, the underweighting factor set by the program according to 
the predicted RSS position error. Used for all measurements except the Doppler 
measurement which uses an underweighting factor of 3 all the time. 

P(I) I = 60, 72 not currently used. 


10.3 THE 3 BY 3 T5 MATRICES 

The T5 matrix converts EF coordinates to TOPodetic coordinates of east, 
north, up as discussed in Section 5.1. 


-TOP 


T5 Rjp 


The T5 matrix is also used to convert the TOP partial derivates of the measure- 
ments back to EF coordinates. The T5 matrix is generated by the subroutine 
ST5. 


T51 = coordinate transformation matrix for the first C-band station, 
T52 = coordinate transformation matrix for the second C-band station. 
T53 - coordinate transformation matrix for the S-band station. 
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n. THE NF (I) FLAGS (INTEGERS) 


All NF(I) are set to zero, in Block 1 of Figure 1, when the program is 
first called. Asterisks identify the NF(I) in the following manner. 

No asterisk means flag is computed by the program. 

*premission input. (There currently are none.) 

**real-time program input. 

***both a real-time input and computed by the program. 

**NF(1) identifies data type from the S-band station.'*' 

NF(1) = 0 for a north-south keyhole. 

NF(1) =1 for an east-west keyhole. 

**NF(2) is the ID number for the first C-band station. 

NF(3) = last value o'*' NF(2). If NF(2) changes from its previous value, then 
the measurement biases and the state error covariance matrix are reinitialized. 

**NF(4) is the ID number for the second C-band station. 

NF(5) = last value of NF(4). If MF(4) changes from its previous value, then 
the measurement biases and the state error covariance matrix are reinitialized. 

**NF(6) is the ID number for the S-band station. 

tThere are no flags for the two data types from the two C-band stations. In- 
stead, the modulus of refraction for the C-band station is checked as follows: 

Nq f 0 indicates normal, . , A, E data. 

= 0 indicates p, A, E data have been corrected for refraction and speed of 
light. 
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NF(7) = last value of NF{6). If NF(6) changes from its previous value, then 
the measurement biases and the state error covariance matrix are reinitialized, 
and NF(18) is set to zero so as to cause the Doppler integration constant to 
be reinitialized. 

***NF(8) = 1, p from first C-band station has a good data tag. 

= 0, p from first C-band station has a bad data tag. 

= -1, p from first C-band station has failed residual edit. 

= -2, p from first C-band station is being manually edited by the 

control board operator. Residuals will continue to be calculated 
in this case. 

***NF(9) = 1, A from first C-band station has a good data tag. 

= 0, A from first C-band station has a bad data tag. 

= -1 , A from first C-band station has failed residual edit. 

= t 2, A from first C-band station is being manually edited by the 

control board operator. Residuals will continue to be calculated in 
this case. 

***NF(10) = 1, E from first C-band station has a good data tag. 

= 0, E from first C-band station has a bad data tag. 

= -1 , E from first C-band station has failed residual edit. 

= -2, E from first C-band station is being manually edited by the 

control board operator. Residuals will continue to be calculated 
in this case. 

*^**NF(11) = 1, p from second C-band station has a good data tag. 

= 0, p from second C-band station has a bad data tag. 

= -1 , p from second C-band station has failed residual edit. 

= -2, p from second C-band station is being manually edited by 

the control board operator. Residuals will continue to be calculated 
in this case. 

^No data or missing data will be tagged bad. 


52 



***NF(12) 


***NF{13) 


***NF(14) 


***NF(15) 


= V, A from second C-band station has a good data tag. 

= 0, A from second C-band station has a bad data tag. 

= -1, A from second C-band station has failed residual edit. 

= -2, A from second C-band station is being manually edited by the 

control board operator. Residuals will continue to be calculated 
in this case. 

= 1, E from second C-band station has a good data tag. 

= 0, E from second C-band station has a bad data tag. 

= -1, E from second C-band station has failed residual edit. 

= -2, E from second C-band station fs being manually edited by the 

control board operator. Residuals will continue to be calculated 
in this case. 

= 1, p from S-band station has a good data tag. 

= 0, p from S-band station has a bad data tag. 

= -1, p from. S-band station has a failed residual edit. 

= -2, p from S-band station is being manually edited by the control 

board operator. Residuals will continue to be calculated in this 
case. ' ■ 

= 1,0^^ from S-band station has a good data tag. 

=■■ 0, from S-band station has a bad data tag. 

- -1, from S-band station has failed residual edit. 

= -2, ay^ from S-band station is being manually edited by the control 
board operator. Residuals will continue to be calculated in this 
case. 
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***NF(16) = 1 , Oy from S-band station has a good data tag. 

= 0, ay from S-band station has a bad data tag. 

= -1’ Oy from S-band station has failed residual edit. 

= -2, oy from S-band station is being manually edited by the control 
board operator. Residuals will continue to be calculated in this 
case. 

***NF(17) = 1, Doppler count from S-band station has a good data tag. 

= 0, Doppler count from S-band station has a bad data tag. 

= -1, Doppler count from S-band station has failed residual edit. 

= -2, Doppler count from S-band station is being manually edited by 
the control board operator. Residuals will continue to be calculated 
in this case. 

NFC18) ■ previous value of NF(17) except that NF(18) cannot equal -1. 

Initially set to zero by the program. IF NF(17) = 1 and NF(18) i 1 then the 
Doppler integration constant is reset and the state error covariance matrix for 
the integration constant is reinitialized. 

**NF(19) = 0, restart from tracking data. (Initially zeroed by the orogram). 

= 1, restart from telemetry vector. 

= 2 j program has been initialized and is running normally. Push button 
indicator (FBI) is turned off. 

**NF(20) = 0 for ascent. 

= 1 for entry 

NF(I) I = 21, 36 not currently used. 
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12. THE FORTRAN LISTING 


12.1 THE MAIN PROGRAM 

The main program inplements the flow diagram shown in Figure 1. Note 
that all constants and variables are placed in unlabled common storage. The 
double precision storage requires 1860 words. The T(19, 19) matrix is used 
for temporary storage. X(19) is the state vector, and C(19, 19) is the state 
error covariance matrix. Note that all comments in the FORTRAN listing 
preceded by an asterisk are programming instructions for the real-time 
programmer. 
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PROGRAM MAIN . _.03/01/.77... 00.35. 15*.PAGE 1. 

OP-1 


1 PROGRAM MAIN _ __ . - - : i 

2 '"DnuBLE PRECISION PS» P» C»T » T51> T52. f 53 J 

3 COMMON PS (721 .P(72)»X(iq», CUV, 191,1(19,19), T5K3.3>»T52I3»3>> I 

A T53(3,3),Nf (36) .. _ ! 

5’ c' ■' aIl comments preceoed by an asterisk are programing instructions for the s 

6 c real time program. I 

r_ CALL BLKl . . f 

" e ' 100 CONTINUE 

9 C ♦IF A RESTART IS CALLED FOR BY THE CONSOLE OPERATOR, THEN SET NF(19)-0 TO i 

_10 C_ ♦RESTART FROM RAW TRACKING DATA, SET NF (.19 )•! TO REST ART_FR_OH A. .TELEMETRY 

11 ■ ■''C ■"" ♦VECTOR. 

12 1F(SF(19) .EQ.2)G0 TO 200 

13 C ♦FETCH REAL TIME IN HOURS AND STORE IN P(5). . 

"19 " I-(P(5)-P(9) )/P(2) 

15 P(l)-I9P(2)-P(2) 

16 200 P(l)-P(l)+P(2) ^ __ 

17' “ CALL ■0LK2 

V’ 18 CALL BLK3 ' 

19 TF(NF(19) .E0.2)CALL 0LK9 _ _ 1 

- '20 ■ ' ■ IF(NF(19) ,EO.O)CALL BLK5 " i 

21 IF(NF(19) .E0.1)CALL BLK6 i 

22 _ call BLK7 _ _ . _ 

'23 ‘C " ♦PROGRAM EXIT/RETURN POINT. ENTER EVERY DELTA T-P(2) HOURS. 

29 GO TO 100 

.25 END ... .. . 





12.2 THE BLK SUBROUTINES 

Each numbered block in the Figure 1 flow diagram is a subroutine. There 
are seven blocks. The FORTRAN instructions for these subroutines are given 
here. 
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SUBROUTINE BLKl 03/01/77. 00. 35. 16. PAGE, I 

OP-1 

1__ subroutine BLKl 

'2 DOUBLE PRECISION PS » P, X » C » T> T51, T52» T53 

3 COMNQN PS(72)#P(72),X( 19),C(17»19),T(19»19»»T51(3>3)»T52I3*3)» 

.. . A T53(3.3).NF(36) .. 

' C AIL COMflENTS PRECEOFD BY AN ASTERISK ARE PROGRAMING INSTRUCTIONS FDR THE 

6 C REAL TIME PROGRAM. 

_7 _ C THIS BLOCK MAY BE EXECUTED AT ANY TIME PRIOR TO ENTERING THE KALMAN . .. 

"e ' “‘c FILTER LOOP. 

9 DO 100 1-1^72 

10 PS(I)-0.00 .. .. 

“il' 100 P( I) -0.00 

12 DO 200 r-1^36 

13 200 NF(I»-0 __ 

~l<t' C *PFAD IN PROGRAM INPUTS TO THE PS ( I ) ANO"P(‘U ARRAYS. V 

15 C *FETCH THE RNP IMAT.PJX FROM CORE AN COLUMN STORE IN P» IT I -22f 30-.-' FE«H- ITS- ^ ^ ^ 

_16 C *TIMF TAG FROM CORE AND STORE IN P(21». _ 

^ 17 C ■ *SET ASCENT-ENTRY FLAG, NF(20), TO 1 IF IN ENTRY PHASE. 

CD 18 C NOTE NF(20»-0 (SET ABOVE) INDICATES ASCENT PHASE. 

19 __ C CALCULATE FIXED CONSTANTS IN THE PS(I) AND P(I) ARRAYS. 

20 PS(69)-PS(68)^P(2) 

21 PS(71)-0EXP(-P(2)/PS(70)) 

22 PS(72>«1.D0-PS{71)**2 

23 P(3)-P{2)**2/2.00 

24 IF(NF(20).E0.L)G0 TO 300 

25 P(10)-DEXPC-P(2)/P(6)) 

26 P(ll)-P(7)**2 

27 GO TO 400 

28 300 P(10)-OEXP(-P(2)/P(8)) _ _ 

29 P(ll)-P(9)**2 

30 400 P(12)-P(11)*T1.D0-P(10I**2) 

31 PETURN 

32 end ' 












, SUBROUTINE BLK2 ..... .03/01/77,:..00. 35..1.7.P.AGE I 

OP-1 

_1_ SUBROUTINE BLK? 

Z ' DOUBLE PRECISION P S » P, X/ C> T » T51» T52» T S3 

■3 COMMON PS(72j,P{72),X(19),C(l'?,19)»T(I9,19)/T51(3»3)»T52(3»3)» 

A T53(3»3),NF(36) 

s' "c " all'comments preceded by an asterisk are programing instructions for the 
6 C PFAL time program. 

_7 C ♦FOR TRACKING DATA WITH A TIME TAG OF P(l) HOURS. LOAD STATION ID NUMBERS _ 

8‘ C +INTO NF(2) (NO. 1 C-BAND), NF(9) (NO. 2 C-BAND), AND NF(6I (S-BAND 

9 C ♦STATION). 

10 C ♦PICK UP ALL TRACKING DATA WITH A TIME TAG OF P(U HOURS AND STORE IN P(I) ' 

11 ' C ♦ARRAY I-43.A5 (FOR NO. 1 C-BANDI^ (FOR NO. 2 C-BAND). I-<)9.52 (FOR 

12 C ♦S-BAND STATION), IF NO DATA THEN LOAD NOTHING AND SET OAfA GOOD-BAD (1.0) 

13 _ C *T0 ZERO. SET DATA GOOD-BAD FLAGS TO ONE OR ZERO. DATA GOOD-BAD FLAGS ARE IN , 

C ♦THE NFtI) ARRAY. I-B.iO (NO, i C'^BAND). 1-11.13 (NO. 2 C-BAND). 1-14.17“’ 

15 C *(S-BAND STATION). IF MANUAL EDIT IS ON FOR A STATION, THEN SET ALL THAT 

16 C ♦STATIONS GOOD-BAD FLAGS TO -2. „ _ 

17 RETURN ” ■' ■■■ '" ' 

\0 18 END 



SOOBOUTINE BLK3 03/01/77. 00. 35. 16. PAGE 1 

OP-1 


_ 1 _ 

2 

3 

5' "C 

6 C 

7 

' b ' C 
9 C 

10 
11 
12 

13 __ 

1A’ 

15 

16 

17 

18 
19 

20 “ 

21 

22 

23 

2A C 

25 C 

26 “ ■ 

27 

28 

29 

30 

31 

32 

33 
3A 

35 

36 

37 

38 


SUBROUTINE BLR3 . 

DOUBLE PRECISION PS>P»XjC»TiT5i»T52»T53 

COMMON PS«72).PI72)»X(19).C(19»19).T(19,19I»T51(3»3)#T52(3. 3). 

A T53(3.3).NF(36) 

ALL COMMENTS PRECEDED BY AN ASTERISK ARE PROGRAMING INSTRUCTIONS EUR THE 
REAL TIME PROGRAM. 

IF(NF(2) .EO.NFO ) )G0 TO 300 .... .. 

♦ FOR STATION NF(2).THE NUMBER 1 C-BAND STATION# PICK UP FROM THE’ STATION 

♦ CHAP.ACTERISTICS TABLE PSdl I-l#ll. 

DO 100 1-1.3 . .. 

T(H-PS(I+5)>*2 
PS< I+11)-T(I)*PS(72) 

PS( I*1A)-PS( I+8)**2 _ _ 

X{I+9)-0.D0 

DO 100 J-1.19 • - 

C(T+9#J)-0.D0 . 

IOC CU»I*9)-0.D0 ■ 

DO 200 1-1,3 
200 C(T>9»T#9»-T<I) 

CALL SEF(PSm,PS(2)#PSI3J#P(l7)»PnBil»PS{18)#P§ri9)#PS(20n 
CALL ST5»PS(2).PS(3)#T51) 

NF<3T-NF{2) 

300 IF{NF(A).E0.NF(5»)G0 TO 600 

♦FOR STATION NFIA)# THE SECOND C-BAND STATION, PICK UP FROM THE STATION 

♦ characteristics table psm 1 - 21 , 31 . 

DO 900 1-1,3 
T{ n-PS(I+25»**2 
P5( I+31l-T( Ii^PS(72) 

PS( I+39)-PS(I+28)t^2 
X(I+12)-D.D0 
DO 900 J-1, 19 
C( I*12, J) -O.DO 
900 C( J, I«-12)-0.D0 
DO 500 1-1,3 
500 C( 1+12, 1+12)-T( I) 

CALL SEF(P5(21) ,PS(22),PS(23»,P(17»,PI18),PS( 38),PS(39),PS(90» ) 

CALL ST5(PS(22),PS(23),T52I 
NF(5)-NF(9) 



- 


\ 




\ 




SUBRHUTINE BLK3 .. . 03/01/77. 00, 35 . 10 .PAGE 2. 

OP-1 

_39 600 lF(f4F<6).E0.NF<7) IRETURN .. .. , 

'g' ♦for station nf(6)» the s-bano station* pick up from the station 

C ♦CHARACTERISTICS TABLE PSII) I-^l,52 AND SET THE NFU) FLAG EQUAL TO 0 FOR 

C *A NORTH-SOUTH KEYHOLE, EQUAL TO 1 FOR AN EAST-WEST KEYHOLE. _ 

A3 ■" C *FROii the DATA STORAGE BUFFER, PICK UP THE TRANSMITTER FREQUENCY IN 

AA C ♦CYCLES/SECONO AND STORE IN PS(66) IN UNITS OF CYCLES/HOUR. 

A5 PS(59)-PS(52)t*2 . .. . 

■~A6‘ ■' ■PS{67)-2.D0*PS(65)*PS16A)+PS(66)/P(19) 

A7 C CAUSE DOPPLER INTEGRATION CONSTANT TO BE INITIALIZED. 

_A8 NFC1B)-0 . . 

A9 ' DO 700 1-1,3 

5U TM)-PS (1 + A5)**2 

_5I PS( I + 52)-n 1)^PS(72) „ 

52 ‘ PSn + 55)-PS(I + A0)+*2 

53 X(I+15)-0.D0 

__5A DO 700 J-1,19 _ 

^ 55^ ' C( r+15, J) -O.DO 

56 700 C( J, I*-15)-0.D0 

57 DO BOO 1-1,3 _ 

‘58 300 C(I+15,I+15)-T(I) 

59 CALL SEF(PS(Al),PS(A2),PS(A3),Pa7»,P(18),PS<60),PS(6l)»PS(62)l 

_60__ CALL ST5{PS(A2»,PS(A3),T53) _ _ ; 

61 ■ NF(7)-NF(6I 

62 RETURN 

63 FND 


SUBROUTINE BLK't ..03/01/77, 00.35,20.PAGE 1 

DP-1 

i_ _ SUBROUTINE BLK<» . ! 

2 DOUBLE PRECISION PS»P>X,C»r»T5l,T5Z,T53 

3 COMMON PS(72),PI72)^X(19>,CU9,19l»T(19»19)jT51(3,3)»T52l3»3», 

A T53(3»3»»NF(36) ... .. 

_ • c ' THIS "subroutine oROPAGATES THE STATE VECTOR AND THE STATE ERROR COVARIANCE 


6 C 

7 


MATRIX AHEAD IN TIME DELTA T-P(2» HOURS 
DO 100 1-1,3 

s' 


I3-I^3 

9 


16-1+6 

10 


X ( I» -X ( I) +X( I 3 ) *P < 2 KX { 16 I ♦? < 3 ) 

11 


X( I3)-X(I3)+X(I6)AP(2) 

12 

ICO 

X ( 16 ) -P( 10)*X ( 16) 

13 


DO 200 1-10,18 

"lA 

200 

xm-psi7i)>xm 

15 


DO 30,0 I-i,3 

16 


13- 1+3 

17 ■■ 


16. 1+6 

18 


DO 300 J-1,19 

19 


T(I, J)-C( I, J)+P(2)*C(I3,J)+PC3)AC(16,J) 

20 


T( 13, J)-C ( 13, J)+P<2)*C( 16, J ) 

21 

300 

T( I6,J)-P(10)*C(I6,J) 

22 


DO 500 J-1,19 

Z3 


DO AOO 1-10,18 

2A 

AOO 

T( I,J)-i»S<71)+Cn, J) 

25 

500 

T{19,J)-C(19,J) 

"26 


DO BOO J-1,3 

27 


J3-J+3 

28 


J6-J+6 

29 


DO 800 1-1,19 

30 


IF{J.GT.I)GO TO 600 

31 


C(l,J)-T(l,J)+T(I,J3)*P(2)+T(I,J6)*P(3) 

32 

600 

1F( J3.GT. UGO TO 700 

33 


C( I, J3)-T(I, J3)+TU,J6)+P(2) 

3A 

700 

IF( J6.GT. I)GO TO BOO 

35 


C{I,J6)-T(1,J6)*P(10) 

36 

8C0 

CONTINUE 

37 


DO 900 J-10,IB 

38 


DO 900 I-J, 19 



a\ 


SUBROUTINE BLK4 
OP»l 


...03/01/77.. 00. 3 5,20. P>6E 2_„ 


39 

900 C(I.J)-T( I,J>* 

PS(71) 


<t 0 

■ DO ’lOOO 1-1,19 



91 

on 1000 j-1, I 



92 

1000 c(j,i)-cn,j) 



93 C .ADD STATE NQIS 

E CO'/ARIANCE 

99 

C{7,7>«C<7»7)+ 

P (12) 


95 

C(8,B)-CI8,8)+ 

P(12) 


96 

C(9,9)-C(9,9) + 

P(12) 


97 

C<10.10)«C(10, 

10)+t>S 

(12) 

98 

c(ii,ii)-cm. 

ID + PS 

(13) 

99 

C(12,12)-C(12, 

1 2 ) +» S 

119) 

50 

C(13,13)-C( 13, 

13)*PS 

(32) 

51 

Cn9,19)»CM9, 

19)+PS 

(33) 

52 

CI15,15)-C(15, 

15 ) 9P S 

(39) 

53 

C(15,16)-C(I6, 

16)+PS 

(53) 

59 

’ CU7,17»-C(17, 

17)-,PS 

(59) 

“55- 

‘ CriB,18)»C(18, 

18)+PS 

(55) 

56 

C(19,19»-C(19, 

19)+PS 

(69) 

57 

RETURN 



■"56' 

■"■’ENO 







SUBROUTINE 

OP-i 


0LK5 


03/01 A77 . 00 . 3 5 . 3 P A.GE J. _ 




1 _ 

2 

3 

A 

”5 

6 

7 

■ e" 

9 

10 

'll 

12 

13 

15 

16 

17 

18 
19 

■ 20 
21 
22 

“23 

2<t 

25 

26 ' 

27 

28 

29 

30 

31 

32 

33 
3<> 

35 

36 

37 

38 


SUBROUTINE BLK5 . „ ... ... 

DOUBLE PRECISION PS » P j C »T »T51» T 52» T 53 

COMNDN PS(72)jP(72)^Xli9)jC(19»19»»T( 19»19)»T51(3»3»#T52(3»3)# 

A_ T53(3f3)»NF(36) . 

" THis'SUBRDUTlNE INITIALIZES THE STATE VECTOR FROM THE RAW TRACKING DATA, 
ALL COMMENTS PRECEDED BY AN ASTERISK ARE PROGRAMING INSTRUCTIONS FOR THE 

REAL TIME PROGRAM. , . . 

IF(NF(0)*NF(9)+^NF(IO).NE.3IGO TO 200 
INITIALIZE USING FIRST C-BANO STATION. 

CONVERT R,A.E TO TOPOOETIC COORDINATES OF EAST» NORTH, UP. 

■ TfA)«P(<r3)1'DCOSIP(<i5n 

T(1)»T(<')*DSIN(P(<.«,) ) EAST 

T(2)«T(<.)*DC0S(P«><,)) . NORTH 

T(3)-P(<.3)*DSINCPI«r5n OP 

TIMES T51 TRANSPOSE GIVES EF COUROINATES* 

DO 100 1-1,3 . . _.. „. 

100 XM)-T51{1, n*T(l)+T51(2, I) ♦T(2)*T5ll3,f)»t(3KPS (1 + 17) 

GO TO 000 

200 IF{NF(ll»+NF(12»+NFU3» .NE. 31-30 TO AOO _ ... ... .. . 

INITIALIZE USING SECOND C-BANO STATION. 

Tt4)-P(A6)*0C0S(P(A8)> 

T(1)-T(<H+DSIN(PU7)) .. EAST 

■“TI2)-T(<,)*0C0S(P(<i7n NORTH 

Tni-PI-iGI^DSINIPCieH OP 

TIMES T52 TRANSPOSE GIVES EF COORDINATES. . .. 

DO 300 1-1,3 

300 xm-T52(l,I)*Tnj*T52(2,I»*T(2»+T52{3,I)*T(3)+PS(I + 37> 

GO TO 800 

<.00 lFrMFH<.)+NF(15)>NF(16».NE.3)RcrURN 
INITIALIZE USING S-BAND STATIJN. 

T(A»-P(A9)*0C0S(P(5in 

IFtNFID.EO.lIGO TO 500 ■ 

A NORTH-SOUTH KEYHOLE STATION. 

T( D-TI^iJ+OSINIPISO) ) EAST . _ 

T(2)-P<A9)*0SIN(P(51)» NORTH 

T(3)-T<A)*DC0S(P(50)» UP 

GO TO 600 _ _ 

AN 'EAST-WEST KEYHOLE STATION. 


tt. 


SUBROUTINE BLK5 03/01/7.7. ,00»35.3<i. RAGE 2. 

OP-1 

39 &00 Tm-P{^9)*0SIN(P(51J ) . ...EAST 

~90' T(2)— T(9)*DSIN<P(50) ) NORTH 

91 T(3)-Tt9)*0COS(P(50)) UP 

__92 C., TIMES T53 TRANSPOSE GIVES EF COORDINATES. .. 

93’ 600 60 700 r-l»3 

99 700 X( n-T53(l, I)*Tm+T53I2» I)*T(2»*T53(3^I)*T(3)*PS( I«-59) 

95 800 DO 900 I- 9» 19 .. 

“‘96 900 Xm-O.DO 

97 DO 1000 1-1.361 

96 _ 1000 C(I»-0,DO , 

-99' " 00 1100 i-1.3 

50 C( I. I »-P< 13) 

_51 C< I+3»I>3)-P(155 . . . 

52 " C( l4-6,r + 6)-P(ll) 

53 C( 1+9. I»9)-PS(l+5)*«2 

59 C( 1 + 12. 1 + 12) -°S(I + 25>+»2 

^ ”55 1100 C(I+15.I+15)-PS(I+95)t*2 

C" 56 C 9TUPN OFF P8I RESTART LIGHTS AND COMMANDS TO RESTART. 

57 NF(l9)-2 

56 C CAUSE DOPPLER INTEGRATION CONSTANT TO BE INITIALIZED. 

59 NF( 181-0 

60_ RETURN . .... 

"61 ■ END 


SUBROUTINE BLK6 03/01/77. . 00.35. 50.PA6E 

OP-1 


~Z 

3 

*5 

6 

7 

'a' 

9 

_10L 

11 

12 

13 

1^ 

15 

16 

17 

18 

19 

20 
21 
22 
23“ 

2<i 

25 

26 

27 

28 

29 

30 

31 

32 

33 
3-V 

35 

36 

37 
36 
39 


_ SUBROUTINE 8LK6 .. 

double Precision ps#p»x,c.t.T5i#t52»T53 

COMMON PS(72) >P( 72) »X( 19) >CU9, 19) jTU9»19). 151(3.3). T52( 3.3). 

_ A T53(3»3).NF{36) . .. 

C THIS SUBROUTINE INITIALIZES THE STATE VECTOR FROM ANY CURRENT TELEHETRY 

C VECTOR. 

C_ ALL COMMENTS PRECEDED BY AN ASTERISK ARE PROGRAMING INSTRUCTIONS FOR THE 

C‘ “real TIME PROGRAM. 

C CONVERT M50 TELEMETRY VECTOR TO EF COORDINATES. 

T(7)-P(20 )*(P(1)-P(21) ) 
n 0)-OCOS(T(7) ) 

T(9)-0SIN(T(7) ) 

on 100 1-1.3 . , 

T(I)-P(T + 21)*P(53)+P( I + 2<i)*P(5A)4-P( I + 27)^P(55) 

100 T( I+3)-PC I*21)*P156)+P( J*2<i)*P(57)4^P( I*27J*P( 5e) 

X(l)-nB)*T(l)+T(9)*T(2) . 

X {2)--TT9 )*T (1 )+T (8)*T{2) 

Xf3)-T(3) 

X (<f).T(8)*T( A)+T(9)*T(5)+P( 20)ftX(2) 

X(5)— T(9)*T(A)*T(8) + T(5)-P(20)*X(1) 

X(6)-T(6) 

DO 200 1-7,19 .. .. . . 

200 X(1)-0,DO 

C INITIALIZE STATE ERROR COVARIANCE MATRIX. 

DO 300 T-1.361 
300 CID-O.OO 

DO 400 r-1.3 
C(i.r)-P(i4) 

C(I+3.T+3)-P(15) 

C< 1 + 6. I *6) -PI 11) 

C( I+9.I+0)-PS ( 1+5) *+2 
C(l+12. I+12)-PS( I + 25)+*2 
400 C(I+l5.I+15)-PS(I+45)**2 

C *TURN OFF PBI RESTART LIGHTS AND COMMANDS TO RESTART, 

NF(19)-2 

C CAUSE DOPPLER INTEGRATION CONSTANT TO BE INITIALIZED. 

NF(l8)-0 

RETURN 

END 


KEPRODUCBILJTY^ QP THB 
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SUBROUTINE BLK7 03yol/7.7.._00.3,6.1lxPAGE 1_ 

' OP-1 


_1 ___ SUBROUTINE BLK7 .... - . 

i ■■ ' double' PRECISION PS » P^ X# C» T» T5X» T52 » T53 

3 COMMON PS(72T,PI72),X{ig),C(19»I9»,T(19,19»,T5l(3»3»#T52l3#3)» 

_Ji _ A_ _ _T53(3»3)»NF(36) . . ^ 

s’ ' C'" THIS SUBROUTINE USES ALL GOOD DATA TO UPDATE THE STATE VECTOR* X, AND | 

6 C the STATE ERROR COVARIANCE MATRIX, C. ALSO CALC. ARE WEIGHTED RESIDUALS. f 

7 C SET UNOERWEIGHTING FACTOR. 

8 Pf5'f>«I.2D0 

9 IF(C{l,l)+^G<2,2)+C(3,3).LT.P(3i»)P(59»-l.DO 

10 _ TF(NF(8) .EQ.1.0P.NF(8) .E0.-2)CALL MEASl ^ •« 

’ii" IF(NF(8).E0.1)CALL UPDATE ' 

12 IF(NF(9) .EQ.1.0R.NFI9) .EQ.-2)CALL MEAS2 

13 IF(NF{9>,E0.1)CALL UPDATE _ . .. . ^ ! 

iA IF(NFdO) .E0.1.0R.NF(10).E0,-2)CALL MEAS3 

15 IF(NF(10J.E0,1)CALL UPDATE 

16 IF(NFMl) .EO.l.OR.NFUi) .EQ.-2>CALL HEASA , ' 

"17~ “ ‘ ‘"IF(NF{11).E0.1)CALL UPDATE P 

18 IF(NF(12) .E0.1.0R.NF(12) .EQ.-2 »CALL MEAS5 

19 IF(NFT12).EQ.1ICALL UPDATE „ . ' ' 

20 IF(NF{13) .EQ.1.0R.NF(13).EQ.“2KALL MEAS6 

21 IF(NFfl3).E0.1)CALL UPDATE 

22 IFCNFUS) ,E0.1.0R.NF(l<r» .E(3.-2)CALL MEAS7 L_ 

23’ '• IF(NFflA) .EO.DCALL UPDATE 

2<f IF(NF(15) .EQ.1.0P.NF(15).EQ.-*2)CALL MEAS8 i 

25 IF(NF(15) .EO.DCALL UPDATE _ _ . ^ I 

’26 ■ IF(NF{16».F.0.1.0R.NF(15).EQ.-2)CALL MEAS9 ' ” 1 

27 IF(HF(16) .EO.DCALL UPDATE i 

28 IF(NF(17).E0.1.AND.NF(18).NE.DCALL INITD ' i 

29 C NOTE THAT THIS CAUSES THE DOPPLER INTEGRATION CONSTANT TO BE INiflALIZED f 

30 C AFTER A MANUAL EDIT IS TURNED OFF. THIS PRECAUTIONARY MEASURE IS TAKEN IN j> 

31 C CASE THE DOPPLER DATA GOES FROM .BAD TO GOOD WHILE THE MANUAL EDIT IS 0N._ ^ 

32 NF(18)-NF(17) ... - | 

33 C NOTE THAT NF(18) CAN NEVER EQUAL -1 (RESIDUAL EDIT). NF(18) CAN ONLY HAVE | 

3^ C VALUES OF 1 (DATA GOOD), 0 (DATA BAD), AND -2 (MANUAL EDIT). !' 

35 IF(NFM7) .E0.1.0R.NF(17).F0.~2)CALL MEASIO 

36 IF(NF(17) .EO.DCALL UPDATE . 

37 RETURN 

30 ' END - 


i 

I; 
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12.3 THE UTILITY SUBROUTINES 

There are five utility subroutines. The first is SUBROUTINE SEF (H, 

ALAT, ALON, RE, E, XEF, YEF, ZEF). This subroutine converts altitude, H, 
geodetic latitude, ALAT, and east longitude, ALON, to earth-fixed coordinates 
of XEF, YEF, and ZEF. The equations shown in Section 6 are used in the con- 
version with 

^ = ALAT 
X = ALON 

= earth's equatorial radius 
E = f, the flattening or ellipticity. 

SUBROUTINE STS (ALAT, ALON, T5) generates the TOPodetic coordinate 
transformation matrix T5 shown in Section 5.1. Inputs to STS are 

ALAT = = geodetic latitude, radians 

ALON = X = east longitude, radians 

Subroutines REFRAC calculates the refraction corrections shown in Section 
5.1. Inputs to REFRAC are 

T(IOO) = range, p T(lOl) = sin E 

T(102) = N|^, refraction modulus T(103) = H^, scale height 

The outputs are 

T(104) = Ap, refraction correction adding to range. 

T(105) = aE, refraction correction adding to elevation angle. 

Other scratch storage used by REFRAC is 

T(106) = cos E T(107) = 1,4-10^ N^ T(108) = 2.7-10^ nJ^'^ 
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T(109) = H 


TO 10) = H* Kill) = H*/Rq 

T(112) = RpC-y/sin^ E + 2H /R^ - sin E] 

* ,6 

T(113) = T(112)*[l - 2.7-10^ ^ (cos ^'*0] = Ap/N„ 

0 Kq 0 

Subroutine INITD initializes the Doppler integration constant X(19) and 
assigns a large variance to it in the C matrix. It also zeros the Doppler 
bias frequency counter, P(32). Temporary storage is 

T(IOO) = range, p 

Subroutine UPDATE updates the state vector and state error covariance 
matrix using a current measurement and using the Kalman filter equations shown 
in Section 8. Temporary storage used by this subroutine is 

T(I + 20) = CP^ I = 1, 19 T(15) = y* - y 

T(16) = PCP^ + Q + aQ T(I + 40) = W = CP’^/T(16) 1 = 1,19 

Note that T(15), T(16) and T(I + 20) I = 1, 19 are inputs to this subroutine. 
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_SUBROUTINE SEF 03/01/77. 00. 35. 14. PAGE—... I 

□ P-1 

_1 „ SUflo.QUTINE SEF(H.ALAT/AL0N»RE>E.XEF»YEF,ZEF) ...... 

2 DOUBLE PRECISION H . ALAT. ALON. RE» E » XEF. Y EF» ZEF, ti. T2» T3» T4 

3 C THIS SUBROUTINE CONVERTS ALTITUDE# GEODETIC LATITUOE.ANO EAST LONGITUDE TO 

_4 c earth-fixed cartesian coordinates. 

5 T1-0C0S(ALAT » 

6 T2-(l-E»**2 

_7 TB-Tl + tp+TP + DSINI ALAT)* + 2 

8 T3-RE/DSORT(T3 » 

9 T4-T3*H 

10 XEF-T4*T1*0C0S(ALQN) . . 

'll" YEF-T4*T1*0SIN( ALON) 

12 7FF-(T3*T2+H)*DSIN(ALAT) 

13 _ RETURN _ 

14 ■ END ” 


O 




SUBROUTINE ST5 „ .. 03/01/77. 00. 36.17. PAGE L.._ 

DP-1 

_1 _ SUBROUTINE ST 5 ( AL AT> ALQN, T5 ) _ 

2 DOUBLE PRECISION ALAT.AL0N.T5.il 

3 DIMENSION T5( 3. 3) 

A C __ THIS SUBROUTINE GENERATES THE TOPQOETIC COORD, . TRANSFORMATION MATRIX. ,T5. 

5 C THE FIRST ROW OF T5 IS A UNIT VECTOR IN EF COORD. IN THE EAST DIRECTION. 

6 C THE SECOND ROW OF T5 IS A UNIT VECTOR IN EF COORD. IN THE NORTH DIRECTION. 

_J C _THE THIRD ROW OF T5 IS A UNIT VECTOR IN EF COORD, IN THE_VERT. DIRECTION. , 

8 ■“ ■■ T5(l.H— OSIN(ALON) 

9 15(1.2)- DCDS(ALON) 

_10 T5(1.3)- O.DO 

11 ‘ T5(2,3)- DCOS(ALAT) 

12 T5(3.3)- DSIN(ALAT) 

13 T5(2.D— T5{3,3)*T5(1.2) __ _ _ _ . 

■"lA ■ T5(2,2)-T5(3.3)*T5(1.1) 

15 T5(3,l)-T5(2.3)+T5(1.2) 

16 T5(3.2) — TStP.Bl+TSd.l) 

17" ' RETURN 

t2 18 END 


SUBROUTINE PEFRAC 
OP-1 


03/01/77. 00. 36. 19. PAGE. _ 1 ^ 


_1 SUBROUTINE REFRAC . 

z double precision PS»P.X,C»T»T5l#T52.T53 

3 common PS(72),P(72)>Xll9),C<l9,19),m9»19).T5K3»3).T52(3.3J» 

A T53(3,3) »NF< 36) 

5* 'C' THIS SUBROUTINE CALCULATES THE REFRACTION CORRECTION FOR RANGE, T<10<i), 

6 C AMO THE REFRACTION CORRECTION FUR ELEVATION ANGLE. T(105). 

_7 _ IFCT(IOO) .LT..02DO*P(17).AND.T(i01).LT..0105DO)niOi)-.0105DO . 

8“'“ " T(lO6)-0SORT{l.DO-T(l01)**2) 

9 T(107)-l.<.06*T«102) 

10 T(108)-2.7D7*DSQRT( T(102n* + 3 .. . .. ... 

11 ■■ T(l091-'’a7)* + 2 + T(100)* + 2 + 2,D0*T(100)+Pa7)*Ta01) 

12 T(109)-0S0RT(T(109) )-P(17) 

13 __ T(llO)-(l.DO-OEXP{-T(109)/T(103)n*T(103) 

!<- T(lll)-T(110)/P(17) 

15 T(112)-P(i7)*(DSORT(0AflS(T(lOl)**^2*2.DO*Ti(llin)“T(lOl)) 

16 T(113)-T{112)*(l.DO-T(108)*T(lii)+T(106)*tT(107) ) ... 

17 T( 10A)-T(102)*T(113 ) 

18 T(l05)-(T(113)*m06)/T(110))*mi02)-T(10A)/T(100)) 

19 __ RETURN 

20 END 




SUBROUTINE INITO . . . _ 0.3/01 /77 . . 00 .36 . 37 .P>GE l_ 

OP-1 


1 _ SUBROUTINE INITD . 

~Z^ DOUBLE PRECISION P S j. P . X. C >T. T 5 if T52» T 53 

3 COMMON PS(72),P(72»»X(19)fC(19,19)>m9fl9)»T51(3.3),T52(3»3)» 

A _ T53(3f 3)»NF(36) . . 

5 C fAlS SUBROUTINE RESETS THE INTEGRATION CONSTANT FOR THE INTEGRATED DOPPLER 

6 C MEASUREMENTS. AND ASSIGNS A LARGE VARIANCE TO IT IN THE C MATRIX. IT ALSO 

_ 7__ C ZEROS THE DOPPLER BIAS FREQUENCT COUNTER. P(32). _ . 

“e T(lOO)-0SORT((X(l)-PS(60n + *2f(X(2)-PS<61»)**2+(X(3)-PS(62»»**2) 

9 X( 19»-P(52)fPS{67»*T(100) 

10 _ DO 100 J-1. 19 

11 C(1P.J)-0.D0 

12 100 CU.19)-0.0a 

13 C{ 19.19)-(C<l.l)+C(2.2)+C(3,3n*PS{67)**2 

14" P( 32) -0.00 

15 RETURN ■ 

16 END . 

UJ . , . 


•C ce ^ O' U> (VJ 


SUBROUTINE UPDATE . . ... . 03/01/77. 00.36. 39. PAG.E „. .1_ 

OP-1 

SUBROUTINE UPDATE ... 

DOUBLE PRECISION PS,P,X,C.T,T5l.T52.T53 

COMMON PS( 72) ,P(72> »xa9),C(19, 19 ),TU9.19),T5113.3J,T52(3»3)» 

A T53(3,3).NF(36) 

■ D‘0’i00“l-1.19 

T( I+<tO)»T( I + 20»/T( 16) 

X(I)-X(I)+Tn>A0)*T(15» 

■ DO 100 J-i.I 

Cd* J)-C( I.J)-T(H-^0)*T( J + 20) 

10 100 C( . . . - 

11 *■■■ RETURN 

12 END 
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12.4 THE MEAS SUBROUTINES 


There are ten measurement subroutines, one for each measurement being 
processed by the program. The primary outputs of the subroutines are: 

T(15) =y - y, the residual. 

T(16) = P(59)*(PCp'*') + Q, the predicted residual variance. 

= 3 PCP"*" + Q for the Doppler measurement 

= PCP"^ + Q + aQ, see Kalman Filter Eqs. in Section £. 

P(I + 32) = scaled residual = T(15)/\/T(16) I = 1, 10. 

T(I + 20) = CP"^ I - 1. 19 

Other temporary storage for the measurement subroutines is: 

T(l), T(2), T(3) = 

T(4), T(5), T(6) = 

T(7), T(8), T(9) = TOPodetic partial derivatives 

T(10) = p^, range^ 

T(ll), T(12), T(13) = Earth-Fixed (EF) partial derivatives 

T(14) = y, the estimated measurement 

T(40) = pp/c, speed of light correction for range 
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T{100) = p 


T(101 ) = sin E 


T(102) = Np T(103) » H 3 


T{104) = Ap, refraction correction adding to range. 


T(105) = aE, refraction correction adding to elevation angle. 


Additional scratch storage for each individual MEAS subroutine is contained 
in T(I) I = 60, 67 and is shovm below. 

Additional temporary storage for MEAS2 and MEAS5 is 


T( 6 G) = + Y 


2 2 
V/AJOP ^V/A,T0P 


Additional temporary storage for MEAS3 and MEAS 6 is 


' V^V/A,T0P ^ ”'^V/A,T0P 


■ ‘'^V^V/AJOP ^V/A,T0P 


Additional scratch storage for fiEASS, north-south keyhole, is 


T(60) = jop 


^^^V/A,TOP ^V/A,TOP 


Additional storage for MEAS 8 , east-west keyhole is 


T(62) = 


" V^V/A,T0P ^V/A, 


TOP 


Additional storage for MEAS9, north-south keyhole, is 


T{6o) =y? 


V/A,T0P ^V/A,T0P 


T( 61 ) = oVxJ/fl.TfiP + Z» 


TOP ‘•V/A,T0P 
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4* V- 






■^(62) = Yy^Ajop/ ^^^V/A.TOP ^V/A,TOP 


T(63) = joP^ ^^V/A,T0P ^V/A,TOP 
Additional storaoe for MEAS9, east-west keyhole, is 

^'''v/A,T0P 4/A,T0P 


T(66) ' 

T(67) = Zy/A^Yop/ + ^fp^jQp 

The equations used in the MEAS subroutines may be found in Section 5. The 
Kalman filter equations used in the MEAS subroutines may be found in Section 8. 


T(64) = 
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SUBROUTINE NEASl _ . 03/01/7.7, 00. 36.A1. PAGE. 1 

QP-1 

_i SUBRaUTINE flEASl .. . , . 

DOUBLE PRECISION P S# P, X, C> T , T 51, T 52, T 53 
3 COMMON PS(72),P(72),X(19),C<19,19),T(19,l9),T51(3,3»,r52(3,3>, 

A T53(3,3),NF(36I , . .. 

"5 C THIS SUBROUTINE IS FOR THE RANGE ME ASUREMENT, R, FROM THE FIRST C-8AND 

6 e STATION. 

7 T(l)-xm-PS(10 ) . . 

e’ T(2) -X(2»-PS( 19) 

9 T(3) •X(3)-PS(20) 

_iO T{10O)»0S0RT(T(l>**2+T(2)**2+T(3»**2) 

11 TM1)«T(1)/TU00) 

12 T( 12)-T(2) /T( 100) 

13 _ T(13)-T{3) /T( 100) . .. 

lA ' ' ■ “ T(1A I'TflOOJ+xUO) 

15 IF(PS(A).LT.i.D-6)G0 TO 100 

16 Tf A0)-{Tm*X(A)*T(2)*X<5) + T(3)+X<6) )/P)19) . — 

17 Tf 10H«T51(3,l)*T{ll) + r51(3,2)Artl2)*T51(3,3)*T(13> 

CO 16 T(102)-PS(A) 

19 T{103)«PS (5) .. . ... - . - 

20 CALL REF RAC 

21 T(1A)«T(1A)-T(A0) + T{10<*) 

22 100 T(15)»P(A3)-TUA) . . 

■"23 on 200 1-1,19 

2<i 200 T(I + 20)-C (1,1 >«Tm)+C( I,2)*m2)+C(I,3)*T113) + C( 1,10) 

25 T(16)-P(59)*(T(ll)^T(2ii+n 12)»T(22)*T(13)*T(23I + T(30) ) + PStl5) 

26" Pt33)-t(15)/DS0RT(DABSIT(l6))) 

27 IF ( OABS(P( 33 n .GT.P( 16) >NF( 3I--1 

26 RETURN . . 

*29 FNO 









SU3RQUTINE MEAS2 - . - 03/01/77# 00. 36, 54. PAGE 1 

OP-1 


__1 SUBROUTINE MEAS2 .... 

■ 2 ■ DOUBLE' 'PRECISION >SsP.’X..C.T,T5l.T52,T53 

"B CONMON P5<72).P(72J.,X(19)>Ctl9,,19».T(19j.l9).T51(3.3)»T52<3,3)# 

A A T53(3>3).NFl36) 

■ 5 C ■ THIS^SUBROUTINE IS FOR THE AZIMUTH MEASUREMENT. A. FROM THE FIRST C-BAND 

6 e STATION. ' , 

7_ T(1)-X(1)-PS(10) ’ ‘ ' 

■ 8 ‘ ' T(2»-X(2)-PS(19) ' 

9 T(3)-X(3)-PS{'20) 

_10 t(A)-T51(l.l)*T(l)+T51(l,2)*T{2)+T51(l»3)+T(3» 

11 * T(5»-T51<2.1)*T(l)+T51(2.2)*T{2)fT51(2f3)*T(3) 

12 T(6)-T51{3.1)*Ta)+T51(3»2»*T(2»+T51(3»3)*T(3) 

13 raA)-DATAN2{T(A>,r(5») + X(ll) 

14 T(15)-P(AA)-T(1A) 

15 IF(T(15) .GT. 2. 00)T(15)-T( 15)-6. 28318530800 

16 TI60»-T(A)**2»-T(5)+*2 

17 ■ T(7)-T(5J /T(60) 

sO 10 T(8)— T(A)/T(60) 

19 T(H)-T(7J*T51(l.l>+T(8)*T51(2»l) _ ..... 

20 ' Ta2)-T(7)*T51(1,2)*T(3)AT5H2.2) 

21 T(13>-T(7)*T51(1.3)+T(0»*T51(2.3) 

22 DO 100 1-1.19 ..... 

23 100 T(I + 20)-C( I.U*T(ll)-»^C(l»2)*ni2)+C(I»3)*f(i3)+C< i.Il) 

24 T(16)-°(59)*mil>*T«21)+ni2l*T(22» + Tn3)^T(23I>T(31) ) + PS(16> 

25 Pf34)-T(15I/DS0RT(DABS(T{.L61l)J 

26 IF(0ABSTP(34) ) .&T.P(lbMNF{9)--l 

27 RETURN 

28 END , . 


subroutine 

OP-1 


ME AS 3 


03/01/77. 00, 37. 37. PAGE L 


1 SUBROUTINE MEAS3 . 

*'V - ■ ■ DOUBLE PRECISION PS»P»X.C»T.T51,T52»T53 

3 COMMON PS(72),P (72),X(19),C (19,19), T( 19, 19>#T5U 3,31 »T52(3»3)# 

A T53(3,3),NF(36) ‘ 

■3 C THIS”SUBROUTINE IS FOR THE ELEVATION ANGLE MEASUREMENT,E,FRaH THE 

6 C C-8AND STATION. 

7 T(i)-X(l>-PS(19 ) 

'6' ■ ‘ T(2)-X(2)-PS(19) 

9 T(3)»X(3)-PS{20) 

10 T<A)»T51(l,l)*T(15*T51{l,2)*T(2)+T51(l,3)*T<3> 

"ll" T(5)«T51(2,1)*T(1)+T51(2,2)*T(2)+T31(2,3)*T(3) 

12 T(6»-T51(3,n+T{l)+T5i(3,2)*T(2)*T51(3,3)*T(3) 

13 __ T(10)-T{A)**2*T(5)**2-H(6)* + 2 

1^' ' T(60)-0S0RT(T(A)**2+T(5)+*2) 

15 r(61)-T(10)*T(60) 

16 T(1A»-DATAN2(T(6),T(60) )+X( 12) , 

‘17 ■■ IF(PS(A) .LT,1,D-6)G0 TO 100 

18 T(100)-DSQRT(T(10) ) 

19_ T(101)-T(6)/TI100) 

20' ' T(102)-PS(A) 

21 T(103)-PS(5) 

22 CALL REFRAC 

'23 T(lA)«T(l'i)+TU05) 

2A ICO T(15)»P(A5)-T(1A) 

Zb IF(T(15).GT,2.00)T(15)-T(i5)-b.2B3185308D0 

26' T(7)— (T(A)/T(61))*T(6) 

27 T(8)— (T(5)/T(61) )*T(6) 

28 T(9)»Tf60)/T{10) 

29 T(Il)-T(7)+T3l(l,l)+T(8)*TSl(2,l)*T(9)*T5H3,l) 

30 T{12)-T(7)*T51(1,2)+T(8)*T5U2,2) »T ( 9 ) ♦ T31 ( 3 , 2 ) 

31 T(I3)-TI7)*T5l(l,3)+T(8)*T51t2,3)*Ti9)*T5l(3,3) 

32 DO 200 1-1,19 

33 200 T(l4^20)-C(l,l)*T(ll)+C(l,2)*T(12)tC(I,3)*T(13)+CII,12) 

3A TM6)-P{59)*(mi)*T(21)*Ttl2)*T(22)FT(13)*T(23)4T(32))tPS(17) 

35 P{35l-T(15)/DSQPT(DABSmi6)l) 

36 IF ••'ABS(PI35) ).GT.P(16) )NF(10)— 1 

37 r; I rn 

38 end ■ - 


FIRST 
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SUBROUTINE MEflSA 

np-1 


03 / 01 / 77 , 


1 SUBROUTINE NEAS4 

2 ■ ■■'double precision PS,P,X,C»T,T3i,T52,T53 

3 COMMON PS(72),P(72)»X(19)»Ca^>19)#T(19»19),T51(3>3),T52(3»3l, 

^ A T53(3,3),NF(36) . _ 

V "c '■ thYs subroutine is for the range measurement# r# from the second c-band 
6 C station. 

_ 7 T(1)»X(1)-PS(38 ) 

8 ■■ T(2)-X(2)-PS(39) 

9 T(3)-X(3)*PS(A0) 

10 T{100)»DS0RT(T(l)**2+n2)**2-H(3)**2) . .. 

~11 ■■ ~ T(11)-T(1 )/T(100) 

12 TM2>-T{2)/T(100) 

13 TM3)-T(3 j /nioo) _ 

14" " T(1A)-Tao0)+X(13) ■ ■ 

15 * IFIPS C24) .LT.1.0-6)GO TO 100 

16 T(40)-mi)*X(4)+T{2»*X(5) + m)*X(6))/P(19) 

CO "17 ■ 'T(101)-T52(3»1>*T( ll)+T52(3#2)*T(12)+T52(3,3)'*T{13) 

18 Tf 102)-PS{24) 

19 T(103)-PSI25) _ 

"20 "■ " CALL PEFRAC 

21 T(14)-T(14)-T(A0)^-T(104) 

22 100 T(15)-P(A6)-m<i) 

"2b DO 200 r-l#19 

24 200 T(I>20»-C (I»I)*T(11)*C (l#2)*TU2>4-Cn»3)*T(13H-CI l»13) 

25 T(16)-P(59) + <T(11)*T{21)+T(12)*H22>+T(13»*T(23) + TU3)) + PS(35) 

' 26' ■ P (36 >»TM5) /OSORKOABSII ( 16) ) ) 

27 IF(0A8S(P(36) ) .GT.P(16) )NF( lU—1 

28 RETURN 

■ 29 END 



_ SUBROUTINE 'lEASS . 03/01/77. 00. 39.13 .P AGE. ^ .1 

OP-1 

_ i_ SUBROUTINE MEAS5 . 

2 ' DOUBLE PRECISION P S» P> X# C» T» T51 , T52» T53 

3 CONNQN PS(72),P(72)»X(19),C(19»i9)»T(19»19) »T51(3»3)^T52(3»3), 

_ A T53(3>3),NF(36) 

5‘ C THIS SUBROUTINE IS FDR THE AZI1UTH MEASUREMENT# A# FROM THE SECOND C-BAND 

6 C STATION. 

_7_ T(l)-xm-PS{3B) .. 

8 ■ ' T(2)-X (EI-PSO^) • 

9 T(l)-X(3)-PS('fOJ 

10 T(A)-T52(l#l>*Tm*T52a»2)*n2)*T52(l,3M'T(3I „ 

'll Tf5)-r52(2.1)+T(l)+T52(2,2)*T(2»+T52(2#3)*T(3) 

12 T(6)-T52(3,1)*T(1)+T52«3#2)*n2)+T52(3»3)*TI3I 

13 T(1A)»DATAN2( T{ A ) .T(5) ) + X U<i) . . , 

“lA T(15)-P(A7)-T{1A) 

15 IF(T(15) .GT. 2. D0mi5)-T(15)-6. 20310530800 

16 T(60)-T(A)**2+T(5J**2 _ . _ 

‘17 ■ T(7)-T{5)/T(60> 

18 T{B)--TIA)/T{60) 

19 T(ll)-T(7) + T52(l»l)+T(8)*152(2.1) . . 

20 T(12)-T(7)*T52(1»2)+T{8)*T52(2#2» 

21 T(13)-T(7)*T52(1#3)+T{8)*T52(2»3) 

22 _ 00 100 I-l#19 _ 

23~ 100 T(I*^20>-C(I#l)*T(ll)*C(I#2»*r(12)+C(I#3)*fn3)+C( I»LA) 

2A TU6)-P(59)*(T(ll)*T<2L)+m2)*I(22H-ni3I*T(23)*T{3A) I+PS(36) 

25 P(37).T(15 )/DS0RT(0ABS(TU6n ) 

26 ~ IFff)ABS{P(37)).GT.PMt>))NF(12I--«l 

27 RETURN 

28 END ' 



SUBPOUTINF MEAS6 . . — 03/01/77. . 00. ,39. 59. PAGE 1_ 

OP-1 


Co 

Oi 


1 

2 

3 

4 

■5 ' C 

6 C 

7 

■ '8 

9 

10 

'll 

12 

13 

14 

15 

16 

17 

18 

19 

20 
21 
22 
'23 

24 

25 _ 
26“ 

27 

28 

29 

30 

31 

32 

33 

34 

35 

36 

37 

38 ■ 


SUBPOUTINE HEAS6 . , 

DOUBLE PPECISION P S » P » X, C » F 51, T52» 1 53 

COMMON PS(72),P(72),X(19),C{19,19),TC19,19),T5H3,3),T52( 3,3), 

A _ T53(3,3),NF(36) , . 

" t"his“subroutine is for the eLevation angle, e, from the second c-band 

STATION. 

T(1)-X(1)-PS<38) .. 

T(2>-X(2)-PS(39) 

T(3)-X(3)-PS(40) 

T(4)»T52(l,l)*T(l)»T52(l,2)*T(2)+T52(l,3)tT(3) , 

T( 5>-T52{2.1)*TU)+T52{2,2) ♦T(2) + T52<2,3)*T (3) 
T{6)-T52{3,1)*T(1)+T52{3,2)*T<2)+T52(3,3)*n3) 

T(10)-T(4)+*2+T(5)+*2+TI6)**2 _ . 

Tf60)-DS0RT<T (4 J**2+T(5)**2) 

T(61)-T(10)*T(60) 

T(14)-DATAN2(T(6), T(60) )+X( 15) .. • - 

IF(PS(24) .LT.1.D-6)G0 TO 100 
Tf 100)-OSORT(T(10) ) 

T{10l)-T(6)/T(l00) . ... . 

T( 102) -PS (24) 

T{ 103)-PS{25) 

CALL REFRAC .. 

T{1A)-T(14)+T(105) 

100 T(15)-P(48)-T(14) 

TF (T(15 ).GT.2.D0)T( 15)-T(15)-6,23318530B00 
T«7) — (T(4)/T(61))*T(6) 

T(9)--(T(5)/T(61))*TI6) 

T{9)-T(60)/T(n) 

T( 1 1) «T ( 7)*T 52 ( 1, 1)>T(8)*T5 2(2,1 )+Tr9)*T52( 3,1) 
T(12)-T(7)+T52(1,2)*T(8)*T52(2,2)*T(9)+T52(3,2I 
T(13 )-T(7)tT52(l,3 )+T( 8)*T52(2,3)+T(9)*T52(3, 3) 

DO 200 1-1,19 

200 T( r+20 )-C(T,l)*T( 11 )+C(l,2)*T(12)+C( I,3)*T( 13)*C( I, 15) 

T(16)-P{59)*(T(11)*T(21)+T{12)*T(22)+T(13)+T(23)4T(35))+PS(37) 

P(3fl >-T(15)/DSQRT(DABS (T(16) )) 

IF{DABS(Pf38) ).GT.P(16) )NF(13)— 1 

RETURN 

END ' 




5U8R0UTINF MEAS7 03/01/77. . OOt^O.SStP AGE 1 

' OP-1 


1 SUBROUTINE ■'1EAS7 

2 ~ 'double PRECISION P S » P ^ Xj C» T rT51» T52» T 53 

3 COMMON PS(72)>P(72)i-X(l'?)»C{l9»19)»T(l9»19)?T51(3r3)»T52(3»3)» 

4 _ _A__ T53{3,3I.NF(36) 

5 ' "c" ' THIS 'SUBROUT INE IS FOR THE RANGE MEASUREMENT* R» FROM THE S-BANO STATION. 

6 T(1)«X(1)-PS(60) 

_7 T(2)«X(2>-PS(61) . - 

8 T(3)-X(3)-PS(62) 

9 T(10'0)-DS0RT(T<l)* + 2 + T(2)**2 + T(3I**2l 

10 _ T<11 )-T(l )/T(100) . . 

11 ' T(12)-T(2)/T(100) 

12 T(13)-T(3)/T(100) 

13 T(^0)-(T{l)*X('))+T(2)*X(5)vT(3>*X(6»»/P(19) __ 

It, T<101)-T53C3*l)+T(ll) + T53<3»2»*T(12l + T53(3»3)*f T13J 

15 T( 102)»PS (AA) 

16 T(103)-PS(A5) . 

17 “ ■ call refrac 

18 T( 1A>-T(IOOI-T{AO)+T(10A )*X(16» 

19 T(15)-R(99)-T(1A) ... . 

20“' "' DO 100 I-l»19 

21 100 T{ I + 20)-C( I»l)*T( 11)*CU*2) *T{12)+C( I*3)*T(13I+C( 1*16) 

22 T(16)-P(59)*(T(11)*T(21)*T( 12)*T(22) + T(13J*n23J + T(36))+PS<56J 

'23 ' P(39)-T(15)/DSQRT(DABS(T(16) »» 

Zt, rf{ DABS (P (39 > ».GT.P « 16»)NF( 1A)»-1 

25 RETURN 

'26' END 
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0P»1 


_1 _ SUBROUTINE NEAS8 , - 

"2 - DOUBLE PRECISION PS, P/X,C.T»T5i.T52»T53 

3 COMMON PSI72) ,P (72) ,X(19),C (19,19 >,T( 19,19), T51(3,3),T52{3,3>, 

_ A T53(3,3),NF(36) . 

■ 5 c" "This SUBROUTINE IS for the X angle measurement, alpha X, FROM THE S-BANO 

6 C STATION. 

_7_ T(1)-X<1)-PS(60) ... ... 

'b“ Tt2)-X<2)-PS(61) 

9 T(3)»X(3)-PS(62) 

10 T(A)-T53(1,1)*T(1)+T53(1,Z)*T{2)+T53(1,3)*T(3) .. . 

11 T(5)-T53(2,l)*T(l)-t-T53(2,2)*T(2) + T53(2,3)*T(3) 

12 T(6)-T53(3,1) + T(1)4-T53(3,2)*T(2)+T53(3,3)+T(3) 

13 _ T(10)-T( A)> + 2*T(5)**2*T(6)**2 _ 

'l^' " ■ T(61)»DS0RT(T(9)**2+T(5)**2) 

15 T(lOO)-0S0RT(T(10)) 

16 T( 101)-T( 6)/T(100) 

^ '"17 ■ T(I02)*pS(<iA) 

vr 18 T(103)-PS(A5) 

19 _CALL REFRAC 

"20 IF(NF(1),NE.0)G0 TO 100 

21 T(60)-T(A)**2+T(6)**2 

22 T(1A)-DATAN2(T<A),T(6))-T{10)*T(9)('T(105)/<T(61)*T160))+X(17) 

"23' T<7)«T (6)/T(60) 

2A T(9) .-T('. ) /T(60) 

25 T< ll)-T(7)>T53(l,l)*T(9)*T53(3,l) 

"26 T(12)-T(7)+T53(1,2)+T(9)*T53(3,2) 

27 T(13)-T<7)*T53(1,3)*T(9)*T53(3,3) 

28 GO TO 200 

29 100 T(62)«T(5)**2*-TJ6)+*2 

30 T(1A)-0ATAN2(-T(5),T(6) )+T( 10)*T(5)+T(105)/(T(61)*n62) )+X(17) 

31 T( 8) --T(6) /T(62) 

32' ■ T(9)-T(5)/T(62) 

33 T(ll)-T(e)*T53(2,l)*T(9)+T53(3,i> 

3^. T(12)-T(3)*T53(2,2)+T(9)*T53(3,2) 

35 T(13)-T(8)+T53(2,3)+TC9)*T53(3,3) 

36 200 T( 15)-P(50)-T(1A ) 

37 lF(T(15).GT.2.D0)T(l5)-ni5 )-6.2831853O8D0 _ 

38 ' DO '300 I-1.19 


subroutine NEAS8 _ 03/01/77. 00. ^1.29jRAG 

OP-J 

39 300_J<I + 20)-Cn,l) + T(ll)+CU.2)+T(12)*CII>3»*T(13)*CII»l7) 

90‘ " ' T(16)«P<59)*{T(ll)*T(2i)*T(12)*T(22)>TU3)*T(23>+T<37))*PS(57) 

P(<t0)-T(15J/0S0RT(DABS(m6»)) 

ifZ IF{0iABSXPHO)).GT.P(16))NF(15)— 1 

■^3 ■ RETURN 

END 





subroutine 

OP-1 


NEAS9 


03/01/77, 00. <» 2. <19. PAGE 1 


: SUBROUTINE »1EAS9 

2 DOUBLE PRECISION P S» P , X, C , T» T 51» T52, T53 

3 CONNQN PS(72)»P(72)#X(19),C(19>19)»m9#19)»T51(3^3)»T&2{3,3)» 

- T33(3, 3),NF136) 

*'5 '”C ‘ ThYs* SUBROUTINE IS FOR THE Y ANGLE MEASUREMENT# ALPHA Y# FROM THE S-BANO 

6 C STATION. 

_7 T{l)«X{l)-PS(60) 

B ' T{2)-X(2)>"PS< 61) 

9 T(3)-X{3)-PS(62) 

10 T(^)-T5311#1)*T(1)+T53(1#2)*T(2)+T53(1»3)*T(3) . . 

'll T(5)-T53<2,1)*T(1)+T53<2#2)+T(2)+T53(2»3)*T(3) 

12 Tt6)-T53(3»l)+Tm+T53«3»2)+T(2)+T53(3»3)*Tt3) 

13 T(lO)-TIA)+*2+T(5)t*2+T(6)**2 

'I'f - ■ T(10O)«DS0RT(T(10n " 

15 T(101)-T(6)/T(100) 

16 T( 102)-PS (AA) 

CO 17 ■" T( 103)-PS(A5) ■ ■ 

18 CALL REFR AC 

19 IF(NF(1) .NE.O)GO TO 100 

20 ■ T(60)-DS0RT(T(A)**2+T(6)**2) 

21 T(61)-T(10)*T(60) 

22_^ T(62)-T<5) /DSQRT(T(A)+*2+T(5)*A2) 

"23 " ■ T(63)-T(6)/T(60) 

2A T(1A)-DATAN2{T(5)>T(60) )-T(62)*T(63)+TU05)+X(18) 

25 T(71— (T( A)/H61) )*T(5) 

*26 T(8)»T(60) /T( 10) 

27 . T(9)--m6)/T(61) )*T(5) 

28 GO TO 200 

29 " 100 T<6A)-DS0RT(T{5)**2*T{6)**2) 

30 T(65)-T(10)+T(6A) 

31 T<66)-T(A)/DSQRT(T(A)+*2+Tl5)**2) _ 

32 T(67)-T(6)/T(6A) 

• 33 TUA)-0ATAN2(T( A)#T{6A) )-T(66) + T(67>*T(1O5) + X(10) 

3A T(7)-T(6A)/T(10) _ 

35' T<8)»-TT( 5) /T(65) )*T( A) 

36 T(9)»-(T(6)/T(65) )*T(A) 

37 200 T{ll)-T(7)^T53(l#l)+T(8)*T53(2#l)A-T(9)*T53(3#l) 

38 T(12)-T(7)*T53(1#2)+T(8)*T53<2#2)+T(9j*t53(3#2) — - - 


03/01/77. OO.A2.^9.PAG£ 2 


^ SU3R0UTINE f*EAS9 
0P«l 

39 T(13)-T(7)*T53(1,3)*T(3)*T53(2 

40 T(15>-P(51}-T(14) 

41 IF(T(15).GT.2.DOJT<15)«TU5)-«> 

42 00 300 1*1. 19 

' 43 " 300 TM + 20)-C(I>l»*T(ll) + C(I»2)*T( 

44 Ttl6)-P<59)*(T(ll)*T(21)+m2) 

45 P(41)*T115)/DSORnDABS«T(16J») 

46" ' TF|DABS«P(41)I.GT.P(16))NFU6) 

47 RETURN 

48 END 


»3)*T(9)*T53(3>3) _ 

.28318530800 

12»+C(I»3)*T(13)+C{ I»18) 
*T(22)fT(13)*T(23)+TI38) )+PS(5B) 

— 1 


CO 



SU3P0UTINE MEASIO 
GP-1 


03/01/77, 00,.«>6,06,PAGE JL 


1 SUBROUTINE NEASIO 

Z DOUBLE PRECISION PS,P, X, C»T, T51,T52»T53 

3 COMMON PS ( 72 ) »P(72),X(l9)»Ctl9,19)»m9»19)>T5l(3>3» ,T52(3,3), 

A A T53{3,3),NF(36) . 

■“5 C THIS SUBROUTINE IS TOR THE DOPPLER MEASUREMENT, N CYCLES, FROM THE 

6 C STATION. 

7 _ Ta)-XU1-PS(60) . , , 

8 ' T(2)-X(2)-PS(61) 

9 T(3»«X(3)-PS(62) 

10 Tn00)-DSQRT{T{1)**2+T(2)+A2 + T(3)**2) 

11 ' T(60)-PS(67)/T(100) 

12 T(101)-(TS3(3,1»+T{1)+T53(3,2)*T(2»+T53(3,3>*T(3J )/T(100) 

13 T(102)-PS(9A) ... ... .. . , .. . 

T(103)-PS<<(5) 

CALL REFPAC 

T(40) -tTm*X(A)+T(2l*X(6) + T(3J*X(7) J/P(19) 

T<1A)-PS( &3)+P( 32)*P(2)-PS(67)*(T(100) + TU0A)-T(A0) I+XU9) 
P(32)-P(32)>1.D0 
TM5I-P(52)-T(1A» 

20 ■ T(1D— T(60)*T(1 ) 

21 T<12)— T(60)*T(2> 

22 T(13>— T(60)+T<3) 

23 DO 100 1-1,19 

29 100 Tn + 20)-Ca,l)*Tlll)+C(I,2> + m2>+C(I.3)*T(13) + C{I,19) 

25 T(16)-3.D0*(T(11 >*T(21)+ni2)*Tt22)+T( 13)*T(23>+T(39) )+PS(59) 

26 P(A2)-T(15)/DS9RT(DABSmi6m 

27 IF(0ABS<P(A2n.GT.P(16) )NF(17)«-1 

20 RETURN 

29 END 


» 


lA 

15 

16 

17 

18 
19 


-BAND 



13. MODIFICATION FOR THE REAL-TIME PROGRAM 

NASA/JSC has made several modifications of the program outlined in this 
document. Perhaps the most important modification is a change in the definition 
of six of the nineteen state variables. In an effort to speed up the program, 
when propagating the error covariance matrix ahead in time, the following state 
variables have been changed. 

^F replaced with aT. 

.. .. 2 
Rpp is replaced with ^p AT /2. 

The new equations of motion are 

^F,i “ ^F,i-1 ^ ^^F ^^F 

* 

(^piT^/2), * cxp(-iT/tj)(Rpp4T^/2),_, 

The first nine elements of the state noise vector become 




90 



0 



0 

0 

0 

0 

0 

(c7,aT^ 2)^1 - exD(-2AT/Tg) 
{a^Ll^/2)yf\ - exp(-2AT/Tg) 
{a^hJ^/Z)yJ] - exp{-2AT/Tg) 1,^3 


Thus the standard deviation of the acceleration state noise is scaled by 
&T^/2. Let c, =6 m/sec^ and aT = 0.2 second. Then the new acceleration 

a 

state noise 0 is 

2 

0 ,/ = o, AT /2 = .12 meters 

a a 

= 1.88- 10"® er 

2 

The initial velocity variance is also scaled by AT . V!e had been using 

2 2 2 

an initial velocity variance of (8000 m/sec) = 20.4 er /hr . The new initial 
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velocity variance becomes (8000 LI m)'' = (1600 m)^ = 6.3’10”® er^. The positi 
variances are, of course, unchanged. 

Note also that the speed of light correction terni in the measurement 
equations, pp/c, was 

pp/c = Bv/a,EF ‘ 

It becomes 

pp/c = .Ev»/a,ef ' ^Bv/a.EF 

It is estimated that the new state vector will reduce the Kalman filter 
cycle times by about 10". 
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